feat(autoware_universe_utils): add TimeKeeper to track function's processing time

common/autoware_universe_utils/CMakeLists.txt

@@ -6,6 +6,8 @@ autoware_package()
 
 find_package(Boost REQUIRED)
 
+find_package(fmt REQUIRED)
+
 ament_auto_add_library(autoware_universe_utils SHARED
   src/geometry/geometry.cpp
   src/geometry/pose_deviation.cpp
@@ -16,6 +18,11 @@ ament_auto_add_library(autoware_universe_utils SHARED
   src/ros/marker_helper.cpp
   src/ros/logger_level_configure.cpp
   src/system/backtrace.cpp
+  src/system/time_keeper.cpp
+)
+
+target_link_libraries(autoware_universe_utils
+  fmt::fmt
 )
 
 if(BUILD_TESTING)

common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp

@@ -0,0 +1,176 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_
+
+#include "autoware/universe_utils/system/stop_watch.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <tier4_debug_msgs/msg/time_node.hpp>
+#include <tier4_debug_msgs/msg/time_tree.hpp>
+
+#include <fmt/format.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace autoware::universe_utils
+{
+/**
+ * @brief Class representing a node in the time tracking tree
+ */
+class TimeNode : public std::enable_shared_from_this<TimeNode>
+{
+public:
+  /**
+   * @brief Construct a new Time Node object
+   *
+   * @param name Name of the node
+   */
+  explicit TimeNode(const std::string & name);
+
+  /**
+   * @brief Add a child node
+   *
+   * @param name Name of the child node
+   * @return std::shared_ptr<TimeNode> Shared pointer to the newly added child node
+   */
+  std::shared_ptr<TimeNode> add_child(const std::string & name);
+
+  /**
+   * @brief Get the result string representing the node and its children in a tree structure
+   *
+   * @param prefix Prefix for the node (used for indentation)
+   * @return std::string Result string representing the node and its children
+   */
+  std::string get_result_str(const std::string & prefix = "") const;
+
+  /**
+   * @brief Construct a TimeTree message from the node and its children
+   *
+   * @param time_tree_msg Reference to the TimeTree message to be constructed
+   * @param parent_id ID of the parent node
+   */
+  void construct_time_tree_msg(
+    tier4_debug_msgs::msg::TimeTree & time_tree_msg, const int parent_id = -1);
+
+  /**
+   * @brief Get the parent node
+   *
+   * @return std::shared_ptr<TimeNode> Shared pointer to the parent node
+   */
+  std::shared_ptr<TimeNode> get_parent_node() const;
+
+  /**
+   * @brief Get the child nodes
+   *
+   * @return std::vector<std::shared_ptr<TimeNode>> Vector of shared pointers to the child nodes
+   */
+  std::vector<std::shared_ptr<TimeNode>> get_child_nodes() const;
+
+  /**
+   * @brief Set the processing time for the node
+   *
+   * @param processing_time Processing time to be set
+   */
+  void set_time(const double processing_time);
+
+  /**
+   * @brief Get the name of the node
+   *
+   * @return std::string Name of the node
+   */
+  std::string get_name() const;
+
+private:
+  const std::string name_;
+  double processing_time_{0.0};
+  std::shared_ptr<TimeNode> parent_node_{nullptr};
+  std::vector<std::shared_ptr<TimeNode>> child_nodes_;
+};
+
+/**
+ * @brief Class for tracking and reporting the processing time of various functions
+ */
+class TimeKeeper
+{
+public:
+  /**
+   * @brief Construct a new TimeKeeper object
+   *
+   * @param node Pointer to the ROS2 node
+   */
+  explicit TimeKeeper(rclcpp::Node * node);
+
+  /**
+   * @brief Report the processing times and optionally show them on the terminal
+   *
+   * @param show_on_terminal Flag indicating whether to show the results on the terminal
+   */
+  void report(const bool show_on_terminal = false);
+
+  /**
+   * @brief Start tracking the processing time of a function
+   *
+   * @param func_name Name of the function to be tracked
+   */
+  void start_track(const std::string & func_name);
+
+  /**
+   * @brief End tracking the processing time of a function
+   *
+   * @param func_name Name of the function to end tracking
+   */
+  void end_track(const std::string & func_name);
+
+private:
+  rclcpp::Publisher<tier4_debug_msgs::msg::TimeTree>::SharedPtr
+    processing_time_pub_;                        //!< Publisher for the processing time message
+  std::shared_ptr<TimeNode> current_time_node_;  //!< Shared pointer to the current time node
+  std::shared_ptr<TimeNode> root_node_;          //!< Shared pointer to the root time node
+  autoware::universe_utils::StopWatch<
+    std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
+    stop_watch_;  //!< StopWatch object for tracking the processing time
+};
+
+/**
+ * @brief Class for automatically tracking the processing time of a function within a scope
+ */
+class ScopedStopWatch
+{
+public:
+  /**
+   * @brief Construct a new ScopedStopWatch object
+   *
+   * @param func_name Name of the function to be tracked
+   * @param time_keepr Reference to the TimeKeeper object
+   */
+  ScopedStopWatch(const std::string & func_name, TimeKeeper & time_keepr);
+
+  /**
+   * @brief Destroy the ScopedStopWatch object, ending the tracking of the function
+   */
+  ~ScopedStopWatch();
+
+private:
+  const std::string func_name_;
+  TimeKeeper & time_keepr_;
+};
+
+}  // namespace autoware::universe_utils
+
+#endif  // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_

common/autoware_universe_utils/src/system/time_keeper.cpp

@@ -0,0 +1,136 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/universe_utils/system/time_keeper.hpp"
+
+#include <iostream>
+#include <stdexcept>
+
+namespace autoware::universe_utils
+{
+
+TimeNode::TimeNode(const std::string & name) : name_(name)
+{
+}
+
+std::shared_ptr<TimeNode> TimeNode::add_child(const std::string & name)
+{
+  auto new_child_node = std::make_shared<TimeNode>(name);
+  new_child_node->parent_node_ = shared_from_this();
+  child_nodes_.push_back(new_child_node);
+  return new_child_node;
+}
+
+std::string TimeNode::get_result_str(const std::string & prefix) const
+{
+  std::ostringstream oss;
+  oss << prefix << name_ << " (" << processing_time_ << "ms)\n";
+  for (const auto & child : child_nodes_) {
+    oss << child->get_result_str(prefix + "  ");
+  }
+  return oss.str();
+}
+
+void TimeNode::construct_time_tree_msg(
+  tier4_debug_msgs::msg::TimeTree & time_tree_msg, const int parent_id)
+{
+  auto time_node_msg = std::make_shared<tier4_debug_msgs::msg::TimeNode>();
+  time_node_msg->name = name_;
+  time_node_msg->processing_time = processing_time_;
+  time_node_msg->id = time_tree_msg.nodes.size();
+  time_node_msg->parent_id = parent_id;
+  time_tree_msg.nodes.push_back(*time_node_msg);
+
+  for (const auto & child : child_nodes_) {
+    child->construct_time_tree_msg(time_tree_msg, time_node_msg->id);
+  }
+}
+
+std::shared_ptr<TimeNode> TimeNode::get_parent_node() const
+{
+  return parent_node_;
+}
+std::vector<std::shared_ptr<TimeNode>> TimeNode::get_child_nodes() const
+{
+  return child_nodes_;
+}
+void TimeNode::set_time(const double processing_time)
+{
+  processing_time_ = processing_time;
+}
+std::string TimeNode::get_name() const
+{
+  return name_;
+}
+
+TimeKeeper::TimeKeeper(rclcpp::Node * node) : current_time_node_(nullptr)
+{
+  processing_time_pub_ =
+    node->create_publisher<tier4_debug_msgs::msg::TimeTree>("~/debug/processing_time_ms_detail", 1);
+}
+
+void TimeKeeper::report(const bool show_on_terminal)
+{
+  if (current_time_node_ != nullptr) {
+    throw std::runtime_error(fmt::format(
+      "You must call end_track({}) first, but report() is called", current_time_node_->get_name()));
+  }
+  if (show_on_terminal) {
+    std::cerr << "========================================" << std::endl;
+    std::cerr << root_node_->get_result_str() << std::endl;
+  }
+
+  tier4_debug_msgs::msg::TimeTree time_tree_msg;
+  root_node_->construct_time_tree_msg(time_tree_msg);
+  processing_time_pub_->publish(time_tree_msg);
+
+  current_time_node_.reset();
+  root_node_.reset();
+}
+
+void TimeKeeper::start_track(const std::string & func_name)
+{
+  if (current_time_node_ == nullptr) {
+    current_time_node_ = std::make_shared<TimeNode>(func_name);
+    root_node_ = current_time_node_;
+  } else {
+    current_time_node_ = current_time_node_->add_child(func_name);
+  }
+  stop_watch_.tic(func_name);
+}
+
+void TimeKeeper::end_track(const std::string & func_name)
+{
+  if (current_time_node_->get_name() != func_name) {
+    throw std::runtime_error(fmt::format(
+      "You must call end_track({}) first, but end_track({}) is called",
+      current_time_node_->get_name(), func_name));
+  }
+  const double processing_time = stop_watch_.toc(func_name);
+  current_time_node_->set_time(processing_time);
+  current_time_node_ = current_time_node_->get_parent_node();
+}
+
+ScopedStopWatch::ScopedStopWatch(const std::string & func_name, TimeKeeper & time_keepr)
+: func_name_(func_name), time_keepr_(time_keepr)
+{
+  time_keepr_.start_track(func_name_);
+}
+
+ScopedStopWatch::~ScopedStopWatch()
+{
+  time_keepr_.end_track(func_name_);
+}
+
+}  // namespace autoware::universe_utils

common/autoware_universe_utils/test/src/system/test_time_keeper.cpp

@@ -0,0 +1,52 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/universe_utils/system/time_keeper.hpp"
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <thread>
+
+TEST(system, TimeKeeper)
+{
+  using autoware::universe_utils::ScopedStopWatch;
+  using autoware::universe_utils::TimeKeeper;
+
+  rclcpp::Node node{"sample_node"};
+
+  TimeKeeper time_keeper(&node);
+
+  time_keeper.start_track("main_func");
+
+  {  // funcA
+    ScopedStopWatch ss{"funcA", time_keeper};
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+  }
+
+  {  // funcB
+    ScopedStopWatch ss{"funcB", time_keeper};
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    {  // funcC
+      ScopedStopWatch ss{"funcC", time_keeper};
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+  }
+
+  time_keeper.end_track("main_func");
+  ASSERT_ANY_THROW(time_keeper.report(true));
+}