feat(autoware_processing_time_checker): add a trigger to choice wheth…

…er to output metrics to log folder (#9479) * add output_metrics option. Signed-off-by: xtk8532704 <[email protected]> * move param set from config to launch file. Signed-off-by: xtk8532704 <[email protected]> * fix bug. Signed-off-by: xtk8532704 <[email protected]> --------- Signed-off-by: xtk8532704 <[email protected]>
autowarefoundation · Nov 27, 2024 · 35048a9 · 35048a9
1 parent a5e9d28
commit 35048a9
Show file tree

Hide file tree

Showing 5 changed files with 71 additions and 0 deletions.
diff --git a/system/autoware_processing_time_checker/README.md b/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.
diff --git a/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml b/system/autoware_processing_time_checker/launch/processing_time_checker.launch.xml
@@ -1,7 +1,10 @@
 <launch>
   <arg name="config_file" default="$(find-pkg-share autoware_processing_time_checker)/config/processing_time_checker.param.yaml"/>
+  <!--  # if output_metrics=true, metrics are written to `<ros2_logging_directory>/autoware_metrics/<node_name>-<time_stamp>.json`. -->
+  <arg name="output_metrics" default="false"/>
 
   <node pkg="autoware_processing_time_checker" exec="processing_time_checker_node" name="processing_time_checker" output="screen">
     <param from="$(var config_file)"/>
+    <param name="output_metrics" value="$(var output_metrics)"/>
   </node>
 </launch>
diff --git a/system/autoware_processing_time_checker/package.xml b/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>

diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/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

diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/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