add output_metrics option.

Signed-off-by: xtk8532704 <[email protected]>

system/autoware_processing_time_checker/README.md

@@ -31,6 +31,8 @@ ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml
 
 {{ json_to_markdown("system/autoware_processing_time_checker/schema/processing_time_checker.schema.json") }}
 
+If `output_metrics = true`, the node writes the statics of the processing_time measured during its lifetime to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json` when shut down.
+
 ## Assumptions / Known limits
 
 TBD.

system/autoware_processing_time_checker/config/processing_time_checker.param.yaml

@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    output_metrics: false # if true, metrics are written to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json`.
     update_rate: 10.0
     processing_time_topic_name_list:
       - /control/control_evaluator/debug/processing_time_ms

system/autoware_processing_time_checker/package.xml

@@ -12,6 +12,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_universe_utils</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tier4_debug_msgs</depend>

system/autoware_processing_time_checker/schema/processing_time_checker.schema.json

@@ -6,6 +6,11 @@
     "autoware_processing_time_checker": {
       "type": "object",
       "properties": {
+        "output_metrics": {
+          "description": "If `output_metrics = true`, the node writes the statics of the processing_time measured during its lifetime to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json` when shut down.",
+          "type": "boolean",
+          "default": "false"
+        },
         "update_rate": {
           "type": "number",
           "default": 10,
@@ -20,7 +25,7 @@
           "description": "The topic name list of the processing time."
         }
       },
-      "required": ["update_rate", "processing_time_topic_name_list"]
+      "required": ["output_metrics", "update_rate", "processing_time_topic_name_list"]
     }
   },
   "properties": {

system/autoware_processing_time_checker/src/processing_time_checker.cpp

@@ -14,8 +14,12 @@
 
 #include "processing_time_checker.hpp"
 
+#include <nlohmann/json.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <chrono>
+#include <filesystem>
+#include <fstream>
 #include <string>
 #include <vector>
 
@@ -38,6 +42,7 @@ std::string get_last_name(const std::string & str)
 ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_options)
 : Node("processing_time_checker", node_options)
 {
+  output_metrics_ = declare_parameter<bool>("output_metrics");
   const double update_rate = declare_parameter<double>("update_rate");
   const auto processing_time_topic_name_list =
     declare_parameter<std::vector<std::string>>("processing_time_topic_name_list");
@@ -64,6 +69,7 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op
     // register module name
     if (module_name) {
       module_name_map_.insert_or_assign(processing_time_topic_name, *module_name);
+      processing_time_accumulator_map_.insert_or_assign(*module_name, Accumulator<double>());
     } else {
       throw std::invalid_argument("The format of the processing time topic name is not correct.");
     }
@@ -79,6 +85,7 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op
         processing_time_topic_name, 1,
         [this, &module_name]([[maybe_unused]] const Float64Stamped & msg) {
           processing_time_map_.insert_or_assign(module_name, msg.data);
+          processing_time_accumulator_map_.at(module_name).add(msg.data);
         }));
     // clang-format on
   }
@@ -90,6 +97,54 @@ ProcessingTimeChecker::ProcessingTimeChecker(const rclcpp::NodeOptions & node_op
     this, get_clock(), period_ns, std::bind(&ProcessingTimeChecker::on_timer, this));
 }
 
+ProcessingTimeChecker::~ProcessingTimeChecker()
+{
+  if (!output_metrics_) {
+    return;
+  }
+
+  // generate json data
+  nlohmann::json j;
+  for (const auto & accumulator_iterator : processing_time_accumulator_map_) {
+    const auto module_name = accumulator_iterator.first;
+    const auto processing_time_accumulator = accumulator_iterator.second;
+    j[module_name + "/min"] = processing_time_accumulator.min();
+    j[module_name + "/max"] = processing_time_accumulator.max();
+    j[module_name + "/mean"] = processing_time_accumulator.mean();
+    j[module_name + "/count"] = processing_time_accumulator.count();
+    j[module_name + "/description"] = "processing time of " + module_name + "[ms]";
+  }
+
+  // get output folder
+  const std::string output_folder_str =
+    rclcpp::get_logging_directory().string() + "/autoware_metrics";
+  if (!std::filesystem::exists(output_folder_str)) {
+    if (!std::filesystem::create_directories(output_folder_str)) {
+      RCLCPP_ERROR(
+        this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str());
+      return;
+    }
+  }
+
+  // get time stamp
+  std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+  std::tm * local_time = std::localtime(&now_time_t);
+  std::ostringstream oss;
+  oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S");
+  std::string cur_time_str = oss.str();
+
+  // Write metrics .json to file
+  const std::string output_file_str =
+    output_folder_str + "/autoware_processing_time_checker-" + cur_time_str + ".json";
+  std::ofstream f(output_file_str);
+  if (f.is_open()) {
+    f << j.dump(4);
+    f.close();
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str());
+  }
+}
+
 void ProcessingTimeChecker::on_timer()
 {
   // create MetricArrayMsg

system/autoware_processing_time_checker/src/processing_time_checker.hpp

@@ -15,6 +15,8 @@
 #ifndef PROCESSING_TIME_CHECKER_HPP_
 #define PROCESSING_TIME_CHECKER_HPP_
 
+#include "autoware/universe_utils/math/accumulator.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
@@ -27,6 +29,7 @@
 
 namespace autoware::processing_time_checker
 {
+using autoware::universe_utils::Accumulator;
 using MetricMsg = tier4_metric_msgs::msg::Metric;
 using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
 using tier4_debug_msgs::msg::Float64Stamped;
@@ -35,6 +38,7 @@ class ProcessingTimeChecker : public rclcpp::Node
 {
 public:
   explicit ProcessingTimeChecker(const rclcpp::NodeOptions & node_options);
+  ~ProcessingTimeChecker() override;
 
 private:
   void on_timer();
@@ -44,10 +48,15 @@ class ProcessingTimeChecker : public rclcpp::Node
   rclcpp::Publisher<MetricArrayMsg>::SharedPtr metrics_pub_;
   std::vector<rclcpp::Subscription<Float64Stamped>::SharedPtr> processing_time_subscribers_;
 
+  // parameters
+  bool output_metrics_;
+
   // topic name - module name
   std::unordered_map<std::string, std::string> module_name_map_{};
   // module name - processing time
   std::unordered_map<std::string, double> processing_time_map_{};
+  // module name - accumulator
+  std::unordered_map<std::string, Accumulator<double>> processing_time_accumulator_map_{};
 };
 }  // namespace autoware::processing_time_checker