Merge branch 'beta/v0.3.17' into backport/4729

system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml

@@ -53,7 +53,7 @@
           transient_local: True
 
         /perception/obstacle_segmentation/pointcloud:
-          module: "sensing"
+          module: "perception"
           timeout: 1.0
           warn_rate: 5.0
           type: "sensor_msgs/msg/PointCloud2"

system/system_error_monitor/config/system_error_monitor.param.yaml

@@ -41,6 +41,15 @@
 
         /autoware/vehicle/node_alive_monitoring: default
 
+      manual_control:
+        /autoware/control/control_command_gate/node_alive_monitoring: default
+        /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"}
+
+        /autoware/system/node_alive_monitoring: default
+        /autoware/system/emergency_stop_operation: default
+
+        /autoware/vehicle/node_alive_monitoring: default
+
       external_control:
         /autoware/control/control_command_gate/node_alive_monitoring: default
         /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"}

system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml

@@ -41,6 +41,16 @@
 
         /autoware/vehicle/node_alive_monitoring: default
 
+      manual_control:
+        /autoware/control/control_command_gate/node_alive_monitoring: default
+        /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"}
+        /autoware/control/external_control/external_command_selector/node_alive_monitoring: default
+
+        /autoware/system/node_alive_monitoring: default
+        /autoware/system/emergency_stop_operation: default
+
+        /autoware/vehicle/node_alive_monitoring: default
+
       external_control:
         /autoware/control/control_command_gate/node_alive_monitoring: default
         /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"}

system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp

@@ -56,6 +56,7 @@ struct KeyName
 {
   static constexpr const char * autonomous_driving = "autonomous_driving";
   static constexpr const char * external_control = "external_control";
+  static constexpr const char * manual_control = "manual_control";
 };
 
 class AutowareErrorMonitor : public rclcpp::Node

system/system_error_monitor/src/system_error_monitor_core.cpp

@@ -232,6 +232,7 @@ AutowareErrorMonitor::AutowareErrorMonitor()
 
   loadRequiredModules(KeyName::autonomous_driving);
   loadRequiredModules(KeyName::external_control);
+  loadRequiredModules(KeyName::manual_control);
 
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -460,6 +461,11 @@ void AutowareErrorMonitor::onTimer()
   current_mode_ = current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO
                     ? KeyName::autonomous_driving
                     : KeyName::external_control;
+  if (
+    current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO &&
+    control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) {
+    current_mode_ = KeyName::manual_control;
+  }
 
   updateHazardStatus();
   publishHazardStatus(hazard_status_);

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.

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<std::string, std::string> 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

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 <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <boost/filesystem.hpp>
 #include <boost/process.hpp>
@@ -37,8 +37,12 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options)
 : Node("ntp_monitor", options),
   updater_(this),
   offset_warn_(declare_parameter<float>("offset_warn", 0.1)),
-  offset_error_(declare_parameter<float>("offset_error", 5.0))
+  offset_error_(declare_parameter<float>("offset_error", 5.0)),
+  timeout_(declare_parameter<int>("timeout", 5)),
+  timeout_expired_(false)
 {
+  using namespace std::literals::chrono_literals;
+
   gethostname(hostname_, sizeof(hostname_));
 
   // Check if command exists
@@ -47,15 +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(
@@ -67,7 +71,18 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat)
   std::string pipe2_err_str;
   float offset = 0.0f;
   std::map<std::string, std::string> tracking_map;
-  error_str = executeChronyc(offset, tracking_map, pipe2_err_str);
+  double elapsed_ms;
+
+  // thread-safe copy
+  {
+    std::lock_guard<std::mutex> 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);
@@ -91,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<std::mutex> 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<std::chrono::milliseconds> stop_watch;
+  stop_watch.tic("execution_time");
+
+  std::string error_str;
+  std::string pipe2_err_str;
+  float offset = 0.0f;
+  std::map<std::string, std::string> tracking_map;
+
+  // Start timeout timer for executing chronyc
+  {
+    std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> lock(timeout_mutex_);
+  timeout_expired_ = true;
 }
 
 std::string NTPMonitor::executeChronyc(