From 05f8ad257db57b0ee700783b8f532f0a1d08ac53 Mon Sep 17 00:00:00 2001 From: ito-san <57388357+ito-san@users.noreply.github.com> Date: Wed, 27 Sep 2023 00:03:16 +0900 Subject: [PATCH] fix(ntp_monitor): move chronyc command execution to a timer (#4634) * fix(ntp_monitor): move chronyc command execution to a timer Signed-off-by: ito-san * add newly added parameter timeout to config --------- Signed-off-by: ito-san Co-authored-by: Hiroki OTA --- .../config/ntp_monitor.param.yaml | 1 + .../ntp_monitor/ntp_monitor.hpp | 23 +++++ .../src/ntp_monitor/ntp_monitor.cpp | 93 ++++++++++++++++--- 3 files changed, 104 insertions(+), 13 deletions(-) diff --git a/system/system_monitor/config/ntp_monitor.param.yaml b/system/system_monitor/config/ntp_monitor.param.yaml index db54f70d1ce59..5f46821f98aff 100644 --- a/system/system_monitor/config/ntp_monitor.param.yaml +++ b/system/system_monitor/config/ntp_monitor.param.yaml @@ -3,3 +3,4 @@ server: ntp.nict.jp offset_warn: 0.1 offset_error: 5.0 + timeout: 5 # The chronyc execution timeout will trigger a warning when this timer expires. diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp index f4ee2de666c22..db264d4b7eda2 100644 --- a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp +++ b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp @@ -53,6 +53,16 @@ class NTPMonitor : public rclcpp::Node void checkOffset( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief Timer callback to execute chronyc command + */ + void onTimer(); + + /** + * @brief Timeout callback function for executing chronyc + */ + void onTimeout(); + /** * @brief function to execute chronyc * @param [out] outOffset offset value of NTP time @@ -71,6 +81,19 @@ class NTPMonitor : public rclcpp::Node float offset_warn_; //!< @brief NTP offset(sec) to generate warning float offset_error_; //!< @brief NTP offset(sec) to generate error + int timeout_; //!< @brief Timeout duration for executing chronyc + + rclcpp::TimerBase::SharedPtr timer_; //!< @brief Timer to execute chronyc command + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; //!< @brief Callback Group + std::mutex mutex_; //!< @brief Mutex for output from chronyc command + std::string error_str_; //!< @brief Error string + std::string pipe2_err_str_; //!< @brief Error string regarding pipe2 function call + float offset_; //!< @brief Offset value of NTP time + std::map tracking_map_; //!< @brief Output of chronyc tracking + double elapsed_ms_; //!< @brief Execution time of chronyc command + rclcpp::TimerBase::SharedPtr timeout_timer_; //!< @brief Timeout for executing chronyc + std::mutex timeout_mutex_; //!< @brief Mutex regarding timeout for executing chronyc + bool timeout_expired_; //!< @brief Timeout for executing chronyc has expired or not /** * @brief NTP offset status messages diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 6ccf7a54b940f..0b91b28b2b5f9 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include "system_monitor/system_monitor_utility.hpp" +#include #include #include @@ -37,8 +37,12 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) : Node("ntp_monitor", options), updater_(this), offset_warn_(declare_parameter("offset_warn", 0.1)), - offset_error_(declare_parameter("offset_error", 5.0)) + offset_error_(declare_parameter("offset_error", 5.0)), + timeout_(declare_parameter("timeout", 5)), + timeout_expired_(false) { + using namespace std::literals::chrono_literals; + gethostname(hostname_, sizeof(hostname_)); // Check if command exists @@ -47,18 +51,15 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("NTP Offset", this, &NTPMonitor::checkOffset); -} -void NTPMonitor::update() -{ - updater_.force_update(); + // Start timer to execute top command + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&NTPMonitor::onTimer, this), timer_callback_group_); } void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!chronyc_exists_) { stat.summary(DiagStatus::ERROR, "chronyc error"); stat.add( @@ -70,7 +71,18 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) std::string pipe2_err_str; float offset = 0.0f; std::map tracking_map; - error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + double elapsed_ms; + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str = error_str_; + pipe2_err_str = pipe2_err_str_; + offset = offset_; + tracking_map = tracking_map_; + elapsed_ms = elapsed_ms_; + } + if (!pipe2_err_str.empty()) { stat.summary(DiagStatus::ERROR, "pipe2 error"); stat.add("pipe2", pipe2_err_str); @@ -94,10 +106,65 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = tracking_map.begin(); itr != tracking_map.end(); ++itr) { stat.add(itr->first, itr->second); } - stat.summary(level, offset_dict_.at(level)); - // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + // Check timeout has expired regarding executing chronyc + bool timeout_expired = false; + { + std::lock_guard lock(timeout_mutex_); + timeout_expired = timeout_expired_; + } + + if (!timeout_expired) { + stat.summary(level, offset_dict_.at(level)); + } else { + stat.summary(DiagStatus::WARN, "chronyc timeout expired"); + } + + stat.addf("execution time", "%f ms", elapsed_ms); +} + +void NTPMonitor::onTimer() +{ + // Start to measure elapsed time + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("execution_time"); + + std::string error_str; + std::string pipe2_err_str; + float offset = 0.0f; + std::map tracking_map; + + // Start timeout timer for executing chronyc + { + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = false; + } + timeout_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::seconds(timeout_), std::bind(&NTPMonitor::onTimeout, this)); + + error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + + // Returning from chronyc, stop timeout timer + timeout_timer_->cancel(); + + const double elapsed_ms = stop_watch.toc("execution_time"); + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str_ = error_str; + pipe2_err_str_ = pipe2_err_str; + offset_ = offset; + tracking_map_ = tracking_map; + elapsed_ms_ = elapsed_ms; + } +} + +void NTPMonitor::onTimeout() +{ + RCLCPP_WARN(get_logger(), "Timeout occurred."); + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = true; } std::string NTPMonitor::executeChronyc(