From 37c22cdf96b673c6423f477087b796aaf152d1a6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 30 Jun 2024 20:14:01 +0900 Subject: [PATCH 01/17] feat(autoware_universe_utils): add TimeKeeper to track function's processing time Signed-off-by: Takayuki Murooka --- .../universe_utils/system/time_keeper.hpp | 164 ++++++++++++++++++ .../path_optimizer/common_structs.hpp | 52 ------ .../autoware/path_optimizer/mpt_optimizer.hpp | 5 +- .../include/autoware/path_optimizer/node.hpp | 3 +- .../state_equation_generator.hpp | 7 +- .../src/mpt_optimizer.cpp | 63 +++---- planning/autoware_path_optimizer/src/node.cpp | 63 +++---- .../src/state_equation_generator.cpp | 3 +- 8 files changed, 218 insertions(+), 142 deletions(-) create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp new file mode 100644 index 000000000000..033de5a1fe7c --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -0,0 +1,164 @@ +// 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 + +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +using DetailedStopWatch = autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>; + +class TimeNode : public std::enable_shared_from_this +{ +public: + explicit TimeNode(const std::string & name) : name_(name) {} + + std::shared_ptr add_child(const std::string & name) + { + auto new_child_node = std::make_shared(name); + + // connect to each other of parent/child + new_child_node->parent_node_ = shared_from_this(); + child_nodes_.push_back(new_child_node); + + return new_child_node; + } + + std::string get_result_text() const + { + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << processing_time_; + return name_ + ":= " + ss.str() + " [ms]"; + } + std::shared_ptr get_parent_node() const { return parent_node_; } + std::vector> get_child_nodes() const { return child_nodes_; } + + void set_time(const double processing_time) { processing_time_ = processing_time; } + +private: + const std::string name_{""}; + double processing_time_{0.0}; + std::shared_ptr parent_node_{nullptr}; + std::vector> child_nodes_{}; +}; + +// TODO(murooka) has to be refactored +class TimeTree +{ +public: + std::shared_ptr current_time_node{}; +}; + +class AutomaticStopWatch +{ +public: + AutomaticStopWatch(const std::string & func_name, std::shared_ptr time_tree) + : func_name_(func_name), time_tree_(time_tree) + { + const auto new_time_node = time_tree_->current_time_node->add_child(func_name); + time_tree_->current_time_node = new_time_node; + stop_watch_.tic(func_name_); + } + ~AutomaticStopWatch() + { + const double processing_time = stop_watch_.toc(func_name_); + time_tree_->current_time_node->set_time(processing_time); + time_tree_->current_time_node = time_tree_->current_time_node->get_parent_node(); + } + +private: + const std::string func_name_; + DetailedStopWatch stop_watch_; + std::shared_ptr time_tree_; +}; + +class TimeKeeper +{ +public: + explicit TimeKeeper(rclcpp::Node * node) + { + processing_time_pub_ = + node->create_publisher("~/debug/processing_time_detail", 1); + } + void start_track(const std::string & func_name) + { + manual_stop_watch_map_.emplace( + func_name, std::make_shared(func_name, time_tree_)); + } + void end_track(const std::string & func_name) { manual_stop_watch_map_.erase(func_name); } + void start(const std::string & func_name) + { + // init + stop_watch_ = std::make_shared(); + time_tree_ = std::make_shared(); + time_tree_->current_time_node = std::make_shared("root"); + + // tic + manual_stop_watch_map_.emplace( + func_name, std::make_shared(func_name, time_tree_)); + } + void end(const std::string & func_name) + { + // toc + manual_stop_watch_map_.erase(func_name); + + // show on the terminal + std::string processing_time_str; + get_child_info(time_tree_->current_time_node, 0, processing_time_str); + processing_time_str.pop_back(); // remove last endline. + std::cerr << "========================================" << std::endl; + std::cerr << processing_time_str; + + // publish + std_msgs::msg::String processing_time_msg; + processing_time_msg.data = processing_time_str; + processing_time_pub_->publish(processing_time_msg); + } + AutomaticStopWatch track(const std::string & func_name) const + { + return AutomaticStopWatch{func_name, time_tree_}; + } + void get_child_info( + const std::shared_ptr time_node, const size_t indent_length, + std::string & processing_time_str) const + { + processing_time_str += std::string(indent_length, ' ') + time_node->get_result_text() + '\n'; + for (const auto & child_time_node : time_node->get_child_nodes()) { + get_child_info(child_time_node, indent_length + 1, processing_time_str); + } + } + +private: + rclcpp::Publisher::SharedPtr processing_time_pub_; + + std::shared_ptr stop_watch_; + std::shared_ptr time_tree_; + + std::unordered_map> manual_stop_watch_map_; +}; +} // namespace autoware::universe_utils +#endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp index 7f2688ffaad9..399262f93e19 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/common_structs.hpp @@ -17,7 +17,6 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "autoware/universe_utils/system/stop_watch.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -43,57 +42,6 @@ struct PlannerData double ego_vel{}; }; -struct TimeKeeper -{ - void init() - { - accumulated_msg = "\n"; - accumulated_time = 0.0; - } - - template - TimeKeeper & operator<<(const T & msg) - { - latest_stream << msg; - return *this; - } - - void endLine() - { - const auto msg = latest_stream.str(); - accumulated_msg += msg + "\n"; - latest_stream.str(""); - - if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_optimizer.time"), msg); - } - } - - void tic(const std::string & func_name) { stop_watch_.tic(func_name); } - - void toc(const std::string & func_name, const std::string & white_spaces) - { - const double elapsed_time = stop_watch_.toc(func_name); - *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; - accumulated_time = elapsed_time; - endLine(); - } - - std::string getLog() const { return accumulated_msg; } - - bool enable_calculation_time_info; - std::string accumulated_msg = "\n"; - std::stringstream latest_stream; - - double getAccumulatedTime() const { return accumulated_time; } - - double accumulated_time{0.0}; - - autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; -}; - struct DebugData { // settting diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp index 4303e5c7e05b..3443ab663b64 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp @@ -19,6 +19,7 @@ #include "autoware/path_optimizer/state_equation_generator.hpp" #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" @@ -107,7 +108,7 @@ class MPTOptimizer rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr); + const std::shared_ptr time_keeper_); std::vector optimizeTrajectory(const PlannerData & planner_data); std::optional> getPrevOptimizedTrajectoryPoints() const; @@ -222,7 +223,7 @@ class MPTOptimizer autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; rclcpp::Logger logger_; MPTParam mpt_param_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 359c20f2a4d2..0fb92e27bdbf 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -22,6 +22,7 @@ #include "autoware/path_optimizer/type_alias.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" @@ -65,7 +66,7 @@ class PathOptimizer : public rclcpp::Node // argument variables autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; - mutable std::shared_ptr time_keeper_ptr_{nullptr}; + mutable std::shared_ptr time_keeper_{nullptr}; // flags for some functions bool enable_pub_debug_marker_; diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp index b8162f5ef2d3..1d23d7788dca 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp @@ -18,6 +18,7 @@ #include "autoware/path_optimizer/common_structs.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include "autoware/path_optimizer/vehicle_model/vehicle_model_interface.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include #include @@ -39,9 +40,9 @@ class StateEquationGenerator StateEquationGenerator() = default; StateEquationGenerator( const double wheel_base, const double max_steer_rad, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : vehicle_model_ptr_(std::make_unique(wheel_base, max_steer_rad)), - time_keeper_ptr_(time_keeper_ptr) + time_keeper_(time_keeper) { } @@ -56,7 +57,7 @@ class StateEquationGenerator private: std::unique_ptr vehicle_model_ptr_; - mutable std::shared_ptr time_keeper_ptr_; + mutable std::shared_ptr time_keeper_; }; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index b5fd266006a4..aed57b0de247 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -395,13 +395,13 @@ MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, - const std::shared_ptr time_keeper_ptr) + const std::shared_ptr time_keeper) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), vehicle_info_(vehicle_info), traj_param_(traj_param), debug_data_ptr_(debug_data_ptr), - time_keeper_ptr_(time_keeper_ptr), + time_keeper_(time_keeper), logger_(node->get_logger().get_child("mpt_optimizer")) { // initialize mpt param @@ -411,7 +411,7 @@ MPTOptimizer::MPTOptimizer( // state equation generator state_equation_generator_ = - StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_ptr_); + StateEquationGenerator(vehicle_info_.wheel_base_m, mpt_param_.max_steer_rad, time_keeper_); // osqp solver osqp_solver_ptr_ = std::make_unique(osqp_epsilon_); @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -520,8 +520,6 @@ std::vector MPTOptimizer::optimizeTrajectory(const PlannerData // 8. publish trajectories for debug publishDebugTrajectories(p.header, ref_points, *mpt_traj_points); - time_keeper_ptr_->toc(__func__, " "); - debug_data_ptr_->ref_points = ref_points; prev_ref_points_ptr_ = std::make_shared>(ref_points); prev_optimized_traj_points_ptr_ = @@ -541,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const auto & p = planner_data; @@ -549,14 +547,14 @@ std::vector MPTOptimizer::calcReferencePoints( const double backward_traj_length = traj_param_.output_backward_traj_length; // 1. resample and convert smoothed points type from trajectory points to reference points - time_keeper_ptr_->tic("resampleReferencePoints"); + time_keeper_->start_track("resampleReferencePoints"); auto ref_points = [&]() { const auto resampled_smoothed_points = trajectory_utils::resampleTrajectoryPointsWithoutStopPoint( smoothed_points, mpt_param_.delta_arc_length); return trajectory_utils::convertToReferencePoints(resampled_smoothed_points); }(); - time_keeper_ptr_->toc("resampleReferencePoints", " "); + time_keeper_->end_track("resampleReferencePoints"); // 2. crop forward and backward with margin, and calculate spline interpolation // NOTE: Margin is added to calculate orientation, curvature, etc precisely. @@ -611,8 +609,6 @@ std::vector MPTOptimizer::calcReferencePoints( ref_points.resize(mpt_param_.num_points); } - time_keeper_ptr_->toc(__func__, " "); - return ref_points; } @@ -639,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); if (!prev_ref_points_ptr_) { // no fixed point @@ -681,8 +677,6 @@ void MPTOptimizer::updateFixedPoint(std::vector & ref_points) co ref_points.front().curvature = front_point.curvature; ref_points.front().fixed_kinematic_state = front_point.optimized_kinematic_state; } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points) const @@ -697,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -786,8 +780,6 @@ void MPTOptimizer::updateExtraPoints(std::vector & ref_points) c } } } - - time_keeper_ptr_->toc(__func__, " "); } void MPTOptimizer::updateBounds( @@ -796,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -844,8 +836,6 @@ void MPTOptimizer::updateBounds( } } */ - - time_keeper_ptr_->toc(__func__, " "); return; } @@ -1104,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1160,8 +1150,6 @@ void MPTOptimizer::updateVehicleBounds( ref_points.at(p_idx).pose_on_constraints.push_back(vehicle_bounds_pose); } } - - time_keeper_ptr_->toc(__func__, " "); } // cost function: J = x' Q x + u' R u @@ -1169,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1228,7 +1216,6 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( R_sparse_mat.setFromTriplets(R_triplet_vec.begin(), R_triplet_vec.end()); - time_keeper_ptr_->toc(__func__, " "); return ValueMatrix{Q_sparse_mat, R_sparse_mat}; } @@ -1236,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1286,7 +1273,6 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( obj_matrix.hessian = H; obj_matrix.gradient = g; - time_keeper_ptr_->toc(__func__, " "); return obj_matrix; } @@ -1297,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1451,7 +1437,6 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( A_rows_end += N_u; } - time_keeper_ptr_->toc(__func__, " "); return ConstraintMatrix{A, lb, ub}; } @@ -1477,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1510,7 +1495,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const auto lower_bound = toStdVector(updated_const_mat.lower_bound); // initialize or update solver according to warm start - time_keeper_ptr_->tic("initOsqp"); + time_keeper_->start_track("initOsqp"); const autoware::common::osqp::CSC_Matrix P_csc = autoware::common::osqp::calCSCMatrixTrapezoidal(H); @@ -1530,13 +1515,12 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( } prev_mat_n_ = H.rows(); prev_mat_m_ = A.rows(); - - time_keeper_ptr_->toc("initOsqp", " "); + time_keeper_->end_track("initOsqp"); // solve qp - time_keeper_ptr_->tic("solveOsqp"); + time_keeper_->start_track("solveOsqp"); const auto result = osqp_solver_ptr_->optimize(); - time_keeper_ptr_->toc("solveOsqp", " "); + time_keeper_->end_track("solveOsqp"); // check solution status const int solution_status = std::get<3>(result); @@ -1563,8 +1547,6 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const Eigen::VectorXd optimized_variables = Eigen::Map(&optimization_result[0], N_v); - time_keeper_ptr_->toc(__func__, " "); - if (u0) { // manual warm start return static_cast(optimized_variables + *u0); } @@ -1647,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1700,7 +1682,6 @@ std::optional> MPTOptimizer::calcMPTPoints( traj_points.push_back(traj_point); } - time_keeper_ptr_->toc(__func__, " "); return traj_points; } @@ -1708,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( @@ -1723,8 +1704,6 @@ void MPTOptimizer::publishDebugTrajectories( // mpt points const auto mpt_traj = autoware::motion_utils::convertToTrajectory(mpt_traj_points, header); debug_mpt_traj_pub_->publish(mpt_traj); - - time_keeper_ptr_->toc(__func__, " "); } std::vector MPTOptimizer::extractFixedPoints( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 3899867a9dcc..e2dc2254e461 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -38,7 +38,8 @@ std::vector concatVectors(const std::vector & prev_vector, const std::vect return concatenated_vector; } -StringStamped createStringStamped(const rclcpp::Time & now, const std::string & data) +[[maybe_unused]] StringStamped createStringStamped( + const rclcpp::Time & now, const std::string & data) { StringStamped msg; msg.stamp = now; @@ -46,7 +47,7 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } -Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +[[maybe_unused]] Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) { Float64Stamped msg; msg.stamp = now; @@ -86,7 +87,7 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), - time_keeper_ptr_(std::make_shared()) + time_keeper_(std::make_shared(this)) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -120,8 +121,6 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) // parameter for debug info enable_debug_info_ = declare_parameter("option.debug.enable_debug_info"); - time_keeper_ptr_->enable_calculation_time_info = - declare_parameter("option.debug.enable_calculation_time_info"); vehicle_stop_margin_outside_drivable_area_ = declare_parameter("common.vehicle_stop_margin_outside_drivable_area"); @@ -137,7 +136,7 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); mpt_optimizer_ptr_ = std::make_shared( this, enable_debug_info_, ego_nearest_param_, vehicle_info_, traj_param_, debug_data_ptr_, - time_keeper_ptr_); + time_keeper_); // reset planners // NOTE: This function must be called after core algorithms (e.g. mpt_optimizer_) have been @@ -177,9 +176,6 @@ rcl_interfaces::msg::SetParametersResult PathOptimizer::onParam( // parameters for debug info updateParam(parameters, "option.debug.enable_debug_info", enable_debug_info_); - updateParam( - parameters, "option.debug.enable_calculation_time_info", - time_keeper_ptr_->enable_calculation_time_info); updateParam( parameters, "common.vehicle_stop_margin_outside_drivable_area", @@ -220,8 +216,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_ptr_->init(); - time_keeper_ptr_->tic(__func__); + time_keeper_->start(__func__); // check if input path is valid if (!checkInputPath(*path_ptr, *get_clock())) { @@ -267,21 +262,21 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) // 5. publish debug data publishDebugData(planner_data.header); - time_keeper_ptr_->toc(__func__, ""); - *time_keeper_ptr_ << "========================================"; - time_keeper_ptr_->endLine(); - // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time + /* const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); debug_calculation_time_str_pub_->publish(calculation_time_msg); debug_calculation_time_float_pub_->publish( createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); + */ const auto output_traj_msg = autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); + + time_keeper_->end(__func__); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const @@ -319,7 +314,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const auto & input_traj_points = planner_data.traj_points; @@ -339,13 +334,12 @@ std::vector PathOptimizer::generateOptimizedTrajectory( // 4. publish debug marker publishDebugMarkerOfOptimization(optimized_traj_points); - time_keeper_ptr_->toc(__func__, " "); return optimized_traj_points; } std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -372,7 +366,6 @@ std::vector PathOptimizer::optimizeTrajectory(const PlannerData // with model predictive trajectory const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data); - time_keeper_ptr_->toc(__func__, " "); return mpt_traj; } @@ -391,7 +384,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -486,14 +479,12 @@ void PathOptimizer::applyInputVelocity( trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); if (optimized_traj_points.empty()) { return; @@ -553,13 +544,11 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( } else { debug_data_ptr_->stop_pose_by_drivable_area = std::nullopt; } - - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -569,36 +558,33 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos } virtual_wall_pub_->publish(virtual_wall_marker); - time_keeper_ptr_->toc(__func__, " "); } void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { + const auto stop_watch(time_keeper_->track(__func__)); + if (!enable_pub_debug_marker_) { return; } - time_keeper_ptr_->tic(__func__); - // debug marker - time_keeper_ptr_->tic("getDebugMarker"); + time_keeper_->start_track("getDebugMarker"); const auto debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, enable_pub_extra_debug_marker_); - time_keeper_ptr_->toc("getDebugMarker", " "); + time_keeper_->end_track("getDebugMarker"); - time_keeper_ptr_->tic("publishDebugMarker"); + time_keeper_->start_track("publishDebugMarker"); debug_markers_pub_->publish(debug_marker); - time_keeper_ptr_->toc("publishDebugMarker", " "); - - time_keeper_ptr_->toc(__func__, " "); + time_keeper_->end_track("publishDebugMarker"); } std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -654,20 +640,17 @@ std::vector PathOptimizer::extendTrajectory( // debug_data_ptr_->extended_traj_points = // extended_traj_points ? *extended_traj_points : std::vector(); - time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } void PathOptimizer::publishDebugData(const Header & header) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); // publish trajectories const auto debug_extended_traj = autoware::motion_utils::convertToTrajectory(debug_data_ptr_->extended_traj_points, header); debug_extended_traj_pub_->publish(debug_extended_traj); - - time_keeper_ptr_->toc(__func__, " "); } } // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 5aa2ab4ffbfa..308267e96b7e 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - time_keeper_ptr_->tic(__func__); + const auto stop_watch(time_keeper_->track(__func__)); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); @@ -60,7 +60,6 @@ StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( W.segment(i * D_x, D_x) = Wd; } - time_keeper_ptr_->toc(__func__, " "); return Matrix{A, B, W}; } From 3ac2ca94aa06e58c9a46e5b4b0f8394fc803a80e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 30 Jun 2024 20:35:10 +0900 Subject: [PATCH 02/17] update Signed-off-by: Takayuki Murooka --- .../universe_utils/system/time_keeper.hpp | 60 ++++++++++--------- .../test/src/system/test_time_keeper.cpp | 47 +++++++++++++++ .../src/mpt_optimizer.cpp | 24 ++++---- planning/autoware_path_optimizer/src/node.cpp | 16 ++--- .../src/state_equation_generator.cpp | 2 +- 5 files changed, 100 insertions(+), 49 deletions(-) create mode 100644 common/autoware_universe_utils/test/src/system/test_time_keeper.cpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 033de5a1fe7c..67419035ccf0 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -29,9 +29,6 @@ namespace autoware::universe_utils { -using DetailedStopWatch = autoware::universe_utils::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>; - class TimeNode : public std::enable_shared_from_this { public: @@ -73,17 +70,17 @@ class TimeTree std::shared_ptr current_time_node{}; }; -class AutomaticStopWatch +class AutoStopWatch { public: - AutomaticStopWatch(const std::string & func_name, std::shared_ptr time_tree) + AutoStopWatch(const std::string & func_name, std::shared_ptr time_tree) : func_name_(func_name), time_tree_(time_tree) { const auto new_time_node = time_tree_->current_time_node->add_child(func_name); time_tree_->current_time_node = new_time_node; stop_watch_.tic(func_name_); } - ~AutomaticStopWatch() + ~AutoStopWatch() { const double processing_time = stop_watch_.toc(func_name_); time_tree_->current_time_node->set_time(processing_time); @@ -92,7 +89,10 @@ class AutomaticStopWatch private: const std::string func_name_; - DetailedStopWatch stop_watch_; + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + std::shared_ptr time_tree_; }; @@ -102,46 +102,54 @@ class TimeKeeper explicit TimeKeeper(rclcpp::Node * node) { processing_time_pub_ = - node->create_publisher("~/debug/processing_time_detail", 1); - } - void start_track(const std::string & func_name) - { - manual_stop_watch_map_.emplace( - func_name, std::make_shared(func_name, time_tree_)); + node->create_publisher("~/debug/processing_time_ms_detail", 1); } - void end_track(const std::string & func_name) { manual_stop_watch_map_.erase(func_name); } + void start(const std::string & func_name) { // init - stop_watch_ = std::make_shared(); time_tree_ = std::make_shared(); time_tree_->current_time_node = std::make_shared("root"); // tic manual_stop_watch_map_.emplace( - func_name, std::make_shared(func_name, time_tree_)); + func_name, std::make_shared(func_name, time_tree_)); } - void end(const std::string & func_name) + void end(const std::string & func_name, const bool show_on_terminal = false) { // toc manual_stop_watch_map_.erase(func_name); - // show on the terminal + // get result text + const auto & root_node = time_tree_->current_time_node; std::string processing_time_str; - get_child_info(time_tree_->current_time_node, 0, processing_time_str); + get_child_info(root_node->get_child_nodes().front(), 0, processing_time_str); processing_time_str.pop_back(); // remove last endline. - std::cerr << "========================================" << std::endl; - std::cerr << processing_time_str; + + // show on the terminal + if (show_on_terminal) { + std::cerr << "========================================" << std::endl; + std::cerr << processing_time_str << std::endl; + } // publish std_msgs::msg::String processing_time_msg; processing_time_msg.data = processing_time_str; processing_time_pub_->publish(processing_time_msg); } - AutomaticStopWatch track(const std::string & func_name) const + + void start_track(const std::string & func_name) + { + manual_stop_watch_map_.emplace( + func_name, std::make_shared(func_name, time_tree_)); + } + void end_track(const std::string & func_name) { manual_stop_watch_map_.erase(func_name); } + AutoStopWatch track(const std::string & func_name) const { - return AutomaticStopWatch{func_name, time_tree_}; + return AutoStopWatch{func_name, time_tree_}; } + +private: void get_child_info( const std::shared_ptr time_node, const size_t indent_length, std::string & processing_time_str) const @@ -152,13 +160,9 @@ class TimeKeeper } } -private: rclcpp::Publisher::SharedPtr processing_time_pub_; - - std::shared_ptr stop_watch_; std::shared_ptr time_tree_; - - std::unordered_map> manual_stop_watch_map_; + std::unordered_map> manual_stop_watch_map_; }; } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp new file mode 100644 index 000000000000..f54746a03783 --- /dev/null +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -0,0 +1,47 @@ +// 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 + +#include + +#include +#include + +TEST(system, TimeKeeper) +{ + using autoware::universe_utils::TimeKeeper; + + TimeKeeper time_keeper(rclcpp::Node{"sample_node"}); + + time_keeper.start("main_func"); + + { // funcA + const auto auto_stop_watch{time_keeper.track("funcA")}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + { // funcB + const auto auto_stop_watch{time_keeper.track("funcB")}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + { // funcC + const auto auto_stop_watch{time_keeper.track("funcC")}; + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + + ASSERT_ANY_THROW(time_keeper.end("main_func")); +} diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index aed57b0de247..01c322fc4ad5 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -539,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const auto & p = planner_data; @@ -635,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); if (!prev_ref_points_ptr_) { // no fixed point @@ -691,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -788,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -1094,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1157,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1223,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1283,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1462,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1629,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1689,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index e2dc2254e461..4a5713d874c3 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -314,7 +314,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const auto & input_traj_points = planner_data.traj_points; @@ -339,7 +339,7 @@ std::vector PathOptimizer::generateOptimizedTrajectory( std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -384,7 +384,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -484,7 +484,7 @@ void PathOptimizer::applyInputVelocity( void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); if (optimized_traj_points.empty()) { return; @@ -548,7 +548,7 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -563,7 +563,7 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); if (!enable_pub_debug_marker_) { return; @@ -584,7 +584,7 @@ std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -645,7 +645,7 @@ std::vector PathOptimizer::extendTrajectory( void PathOptimizer::publishDebugData(const Header & header) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); // publish trajectories const auto debug_extended_traj = diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 308267e96b7e..b6a3efecd8e1 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - const auto stop_watch(time_keeper_->track(__func__)); + const auto auto_stop_watch(time_keeper_->track(__func__)); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); From b08c03821ce40a8a03848e0918d51532a2c9582c Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Tue, 2 Jul 2024 20:25:31 +0900 Subject: [PATCH 03/17] refactoring Signed-off-by: Y.Hisaki --- common/autoware_universe_utils/CMakeLists.txt | 7 + .../universe_utils/system/time_keeper.hpp | 248 ++++++++++-------- .../src/system/time_keeper.cpp | 141 ++++++++++ .../test/src/system/test_time_keeper.cpp | 17 +- .../src/mpt_optimizer.cpp | 24 +- planning/autoware_path_optimizer/src/node.cpp | 21 +- .../src/state_equation_generator.cpp | 2 +- 7 files changed, 316 insertions(+), 144 deletions(-) create mode 100644 common/autoware_universe_utils/src/system/time_keeper.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 9e86ddeb6069..f032451db235 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/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) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 67419035ccf0..0d84dbf09b6b 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -19,150 +19,168 @@ #include -#include "std_msgs/msg/string.hpp" +#include +#include + +#include #include -#include #include -#include #include namespace autoware::universe_utils { +/** + * @brief Class representing a node in the time tracking tree + */ class TimeNode : public std::enable_shared_from_this { public: - explicit TimeNode(const std::string & name) : name_(name) {} - - std::shared_ptr add_child(const std::string & name) - { - auto new_child_node = std::make_shared(name); - - // connect to each other of parent/child - new_child_node->parent_node_ = shared_from_this(); - child_nodes_.push_back(new_child_node); - - return new_child_node; - } - - std::string get_result_text() const - { - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << processing_time_; - return name_ + ":= " + ss.str() + " [ms]"; - } - std::shared_ptr get_parent_node() const { return parent_node_; } - std::vector> get_child_nodes() const { return child_nodes_; } - - void set_time(const double processing_time) { processing_time_ = processing_time; } + /** + * @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 Shared pointer to the newly added child node + */ + std::shared_ptr 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 Shared pointer to the parent node + */ + std::shared_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child nodes + */ + std::vector> 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_{""}; + const std::string name_; double processing_time_{0.0}; std::shared_ptr parent_node_{nullptr}; - std::vector> child_nodes_{}; + std::vector> child_nodes_; }; -// TODO(murooka) has to be refactored -class TimeTree -{ -public: - std::shared_ptr current_time_node{}; -}; +class ScopedStopWatch; -class AutoStopWatch +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper { public: - AutoStopWatch(const std::string & func_name, std::shared_ptr time_tree) - : func_name_(func_name), time_tree_(time_tree) - { - const auto new_time_node = time_tree_->current_time_node->add_child(func_name); - time_tree_->current_time_node = new_time_node; - stop_watch_.tic(func_name_); - } - ~AutoStopWatch() - { - const double processing_time = stop_watch_.toc(func_name_); - time_tree_->current_time_node->set_time(processing_time); - time_tree_->current_time_node = time_tree_->current_time_node->get_parent_node(); - } + /** + * @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); + + /** + * @brief Create a ScopedStopWatch object to track the processing time of a function + * + * @param func_name Name of the function to be tracked + * @return ScopedStopWatch ScopedStopWatch object for tracking the function + */ + ScopedStopWatch track(const std::string & func_name); private: - const std::string func_name_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; //!< Publisher for the processing time message + std::shared_ptr current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr 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_; - - std::shared_ptr time_tree_; + stop_watch_; //!< StopWatch object for tracking the processing time }; -class TimeKeeper +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedStopWatch { public: - explicit TimeKeeper(rclcpp::Node * node) - { - processing_time_pub_ = - node->create_publisher("~/debug/processing_time_ms_detail", 1); - } - - void start(const std::string & func_name) - { - // init - time_tree_ = std::make_shared(); - time_tree_->current_time_node = std::make_shared("root"); - - // tic - manual_stop_watch_map_.emplace( - func_name, std::make_shared(func_name, time_tree_)); - } - void end(const std::string & func_name, const bool show_on_terminal = false) - { - // toc - manual_stop_watch_map_.erase(func_name); - - // get result text - const auto & root_node = time_tree_->current_time_node; - std::string processing_time_str; - get_child_info(root_node->get_child_nodes().front(), 0, processing_time_str); - processing_time_str.pop_back(); // remove last endline. - - // show on the terminal - if (show_on_terminal) { - std::cerr << "========================================" << std::endl; - std::cerr << processing_time_str << std::endl; - } - - // publish - std_msgs::msg::String processing_time_msg; - processing_time_msg.data = processing_time_str; - processing_time_pub_->publish(processing_time_msg); - } - - void start_track(const std::string & func_name) - { - manual_stop_watch_map_.emplace( - func_name, std::make_shared(func_name, time_tree_)); - } - void end_track(const std::string & func_name) { manual_stop_watch_map_.erase(func_name); } - AutoStopWatch track(const std::string & func_name) const - { - return AutoStopWatch{func_name, time_tree_}; - } + /** + * @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: - void get_child_info( - const std::shared_ptr time_node, const size_t indent_length, - std::string & processing_time_str) const - { - processing_time_str += std::string(indent_length, ' ') + time_node->get_result_text() + '\n'; - for (const auto & child_time_node : time_node->get_child_nodes()) { - get_child_info(child_time_node, indent_length + 1, processing_time_str); - } - } - - rclcpp::Publisher::SharedPtr processing_time_pub_; - std::shared_ptr time_tree_; - std::unordered_map> manual_stop_watch_map_; + const std::string func_name_; + TimeKeeper & time_keepr_; }; + } // namespace autoware::universe_utils + #endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp new file mode 100644 index 000000000000..7a348b158fa4 --- /dev/null +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -0,0 +1,141 @@ +// 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 +#include + +namespace autoware::universe_utils +{ + +TimeNode::TimeNode(const std::string & name) : name_(name) +{ +} + +std::shared_ptr TimeNode::add_child(const std::string & name) +{ + auto new_child_node = std::make_shared(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(); + 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::get_parent_node() const +{ + return parent_node_; +} +std::vector> 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("~/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(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_); +} + +inline ScopedStopWatch TimeKeeper::track(const std::string & func_name) +{ + return ScopedStopWatch(func_name, *this); +} + +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index f54746a03783..e8a3a1589500 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -14,6 +14,7 @@ #include "autoware/universe_utils/system/time_keeper.hpp" +#include #include #include @@ -23,25 +24,29 @@ TEST(system, TimeKeeper) { + using autoware::universe_utils::ScopedStopWatch; using autoware::universe_utils::TimeKeeper; - TimeKeeper time_keeper(rclcpp::Node{"sample_node"}); + rclcpp::Node node{"sample_node"}; - time_keeper.start("main_func"); + TimeKeeper time_keeper(&node); + + time_keeper.start_track("main_func"); { // funcA - const auto auto_stop_watch{time_keeper.track("funcA")}; + ScopedStopWatch ss{"funcA", time_keeper}; std::this_thread::sleep_for(std::chrono::seconds(1)); } { // funcB - const auto auto_stop_watch{time_keeper.track("funcB")}; + ScopedStopWatch ss{"funcB", time_keeper}; std::this_thread::sleep_for(std::chrono::seconds(1)); { // funcC - const auto auto_stop_watch{time_keeper.track("funcC")}; + ScopedStopWatch ss{"funcC", time_keeper}; std::this_thread::sleep_for(std::chrono::seconds(1)); } } - ASSERT_ANY_THROW(time_keeper.end("main_func")); + time_keeper.end_track("main_func"); + ASSERT_ANY_THROW(time_keeper.report(true)); } diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 01c322fc4ad5..74a3efa17a71 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -539,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const auto & p = planner_data; @@ -635,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -691,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -788,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -1094,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1157,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1223,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1283,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1462,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1629,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1689,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 4a5713d874c3..e15915115589 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -216,7 +216,7 @@ void PathOptimizer::resetPreviousData() void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) { - time_keeper_->start(__func__); + time_keeper_->start_track(__func__); // check if input path is valid if (!checkInputPath(*path_ptr, *get_clock())) { @@ -276,7 +276,8 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) traj_pub_->publish(output_traj_msg); published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); - time_keeper_->end(__func__); + time_keeper_->end_track(__func__); + time_keeper_->report(true); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const @@ -314,7 +315,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -339,7 +340,7 @@ std::vector PathOptimizer::generateOptimizedTrajectory( std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -384,7 +385,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -484,7 +485,7 @@ void PathOptimizer::applyInputVelocity( void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -548,7 +549,7 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -563,7 +564,7 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); if (!enable_pub_debug_marker_) { return; @@ -584,7 +585,7 @@ std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -645,7 +646,7 @@ std::vector PathOptimizer::extendTrajectory( void PathOptimizer::publishDebugData(const Header & header) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index b6a3efecd8e1..5f07cc5ea377 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - const auto auto_stop_watch(time_keeper_->track(__func__)); + autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); From 55ea36579ebb0352dd5979f9f8bd152b89d2fdf3 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Tue, 2 Jul 2024 20:31:10 +0900 Subject: [PATCH 04/17] remove track Signed-off-by: Y.Hisaki --- .../autoware/universe_utils/system/time_keeper.hpp | 10 ---------- .../autoware_universe_utils/src/system/time_keeper.cpp | 5 ----- 2 files changed, 15 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 0d84dbf09b6b..56a9fd101814 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -103,8 +103,6 @@ class TimeNode : public std::enable_shared_from_this std::vector> child_nodes_; }; -class ScopedStopWatch; - /** * @brief Class for tracking and reporting the processing time of various functions */ @@ -139,14 +137,6 @@ class TimeKeeper */ void end_track(const std::string & func_name); - /** - * @brief Create a ScopedStopWatch object to track the processing time of a function - * - * @param func_name Name of the function to be tracked - * @return ScopedStopWatch ScopedStopWatch object for tracking the function - */ - ScopedStopWatch track(const std::string & func_name); - private: rclcpp::Publisher::SharedPtr processing_time_pub_; //!< Publisher for the processing time message diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 7a348b158fa4..93255d2a764c 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -133,9 +133,4 @@ ScopedStopWatch::~ScopedStopWatch() time_keepr_.end_track(func_name_); } -inline ScopedStopWatch TimeKeeper::track(const std::string & func_name) -{ - return ScopedStopWatch(func_name, *this); -} - } // namespace autoware::universe_utils From 45a2198bb006acc094f6e62e5a29833c0e582dd4 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Thu, 4 Jul 2024 14:31:20 +0900 Subject: [PATCH 05/17] refactor Signed-off-by: Y.Hisaki --- .../universe_utils/system/time_keeper.hpp | 69 +++++++------- .../src/system/time_keeper.cpp | 89 ++++++++++++------- .../test/src/system/test_time_keeper.cpp | 14 +-- .../src/mpt_optimizer.cpp | 24 ++--- planning/autoware_path_optimizer/src/node.cpp | 16 ++-- .../src/state_equation_generator.cpp | 2 +- 6 files changed, 117 insertions(+), 97 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 56a9fd101814..b06f55c43cbf 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -11,7 +11,6 @@ // 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_ @@ -19,10 +18,8 @@ #include -#include -#include - -#include +#include +#include #include #include @@ -33,54 +30,52 @@ namespace autoware::universe_utils /** * @brief Class representing a node in the time tracking tree */ -class TimeNode : public std::enable_shared_from_this +class ProcessingTimeNode : public std::enable_shared_from_this { public: /** - * @brief Construct a new Time Node object + * @brief Construct a new ProcessingTimeNode object * * @param name Name of the node */ - explicit TimeNode(const std::string & name); + explicit ProcessingTimeNode(const std::string & name); /** * @brief Add a child node * * @param name Name of the child node - * @return std::shared_ptr Shared pointer to the newly added child node + * @return std::shared_ptr Shared pointer to the newly added child node */ - std::shared_ptr add_child(const std::string & name); + std::shared_ptr 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; + std::string to_string() const; /** - * @brief Construct a TimeTree message from the node and its children + * @brief Construct a ProcessingTimeTree 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 + * @return tier4_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree message */ - void construct_time_tree_msg( - tier4_debug_msgs::msg::TimeTree & time_tree_msg, const int parent_id = -1); + tier4_debug_msgs::msg::ProcessingTimeTree to_msg() const; /** * @brief Get the parent node * - * @return std::shared_ptr Shared pointer to the parent node + * @return std::shared_ptr Shared pointer to the parent node */ - std::shared_ptr get_parent_node() const; + std::shared_ptr get_parent_node() const; /** * @brief Get the child nodes * - * @return std::vector> Vector of shared pointers to the child nodes + * @return std::vector> Vector of shared pointers to the child + * nodes */ - std::vector> get_child_nodes() const; + std::vector> get_child_nodes() const; /** * @brief Set the processing time for the node @@ -97,10 +92,11 @@ class TimeNode : public std::enable_shared_from_this std::string get_name() const; private: - const std::string name_; - double processing_time_{0.0}; - std::shared_ptr parent_node_{nullptr}; - std::vector> child_nodes_; + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::shared_ptr parent_node_{nullptr}; //!< Shared pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes }; /** @@ -138,10 +134,11 @@ class TimeKeeper void end_track(const std::string & func_name); private: - rclcpp::Publisher::SharedPtr - processing_time_pub_; //!< Publisher for the processing time message - std::shared_ptr current_time_node_; //!< Shared pointer to the current time node - std::shared_ptr root_node_; //!< Shared pointer to the root time node + rclcpp::Publisher::SharedPtr + processing_time_pub_; //!< Publisher for the processing time message + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr 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 @@ -150,25 +147,25 @@ class TimeKeeper /** * @brief Class for automatically tracking the processing time of a function within a scope */ -class ScopedStopWatch +class ScopedTimeTrack { public: /** - * @brief Construct a new ScopedStopWatch object + * @brief Construct a new ScopedTimeTrack 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); + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keepr); /** - * @brief Destroy the ScopedStopWatch object, ending the tracking of the function + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function */ - ~ScopedStopWatch(); + ~ScopedTimeTrack(); private: - const std::string func_name_; - TimeKeeper & time_keepr_; + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keepr_; //!< Reference to the TimeKeeper object }; } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 93255d2a764c..91f24886e8d1 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -14,70 +14,95 @@ #include "autoware/universe_utils/system/time_keeper.hpp" +#include + #include #include namespace autoware::universe_utils { -TimeNode::TimeNode(const std::string & name) : name_(name) +ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) { } -std::shared_ptr TimeNode::add_child(const std::string & name) +std::shared_ptr ProcessingTimeNode::add_child(const std::string & name) { - auto new_child_node = std::make_shared(name); + auto new_child_node = std::make_shared(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::string ProcessingTimeNode::to_string() const { + std::function + construct_string = [&]( + const ProcessingTimeNode & node, std::ostringstream & oss, + const std::string & prefix, bool is_last, bool is_root) { + if (!is_root) { + oss << prefix << (is_last ? "└── " : "├── "); + } + oss << node.name_ << " (" << node.processing_time_ << "ms)\n"; + for (size_t i = 0; i < node.child_nodes_.size(); ++i) { + const auto & child = node.child_nodes_[i]; + construct_string( + *child, oss, prefix + (is_last ? " " : "│ "), i == node.child_nodes_.size() - 1, + false); + } + }; + std::ostringstream oss; - oss << prefix << name_ << " (" << processing_time_ << "ms)\n"; - for (const auto & child : child_nodes_) { - oss << child->get_result_str(prefix + " "); - } + construct_string(*this, oss, "", true, true); return oss.str(); } -void TimeNode::construct_time_tree_msg( - tier4_debug_msgs::msg::TimeTree & time_tree_msg, const int parent_id) +tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const { - auto time_node_msg = std::make_shared(); - 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); - } + tier4_debug_msgs::msg::ProcessingTimeTree time_tree_msg; + + std::function + construct_msg = [&]( + const ProcessingTimeNode & node, + tier4_debug_msgs::msg::ProcessingTimeTree & tree_msg, int parent_id) { + tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; + time_node_msg.name = node.name_; + time_node_msg.processing_time = node.processing_time_; + time_node_msg.id = tree_msg.nodes.size(); + time_node_msg.parent_id = parent_id; + tree_msg.nodes.emplace_back(time_node_msg); + + for (const auto & child : node.child_nodes_) { + construct_msg(*child, tree_msg, time_node_msg.id); + } + }; + construct_msg(*this, time_tree_msg, 0); + + return time_tree_msg; } -std::shared_ptr TimeNode::get_parent_node() const +std::shared_ptr ProcessingTimeNode::get_parent_node() const { return parent_node_; } -std::vector> TimeNode::get_child_nodes() const +std::vector> ProcessingTimeNode::get_child_nodes() const { return child_nodes_; } -void TimeNode::set_time(const double processing_time) +void ProcessingTimeNode::set_time(const double processing_time) { processing_time_ = processing_time; } -std::string TimeNode::get_name() const +std::string ProcessingTimeNode::get_name() const { return name_; } TimeKeeper::TimeKeeper(rclcpp::Node * node) : current_time_node_(nullptr) { - processing_time_pub_ = - node->create_publisher("~/debug/processing_time_ms_detail", 1); + processing_time_pub_ = node->create_publisher( + "~/debug/processing_time_ms_detail", 1); } void TimeKeeper::report(const bool show_on_terminal) @@ -88,12 +113,10 @@ void TimeKeeper::report(const bool show_on_terminal) } if (show_on_terminal) { std::cerr << "========================================" << std::endl; - std::cerr << root_node_->get_result_str() << std::endl; + std::cerr << root_node_->to_string() << 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); + processing_time_pub_->publish(root_node_->to_msg()); current_time_node_.reset(); root_node_.reset(); @@ -102,7 +125,7 @@ void TimeKeeper::report(const bool show_on_terminal) void TimeKeeper::start_track(const std::string & func_name) { if (current_time_node_ == nullptr) { - current_time_node_ = std::make_shared(func_name); + current_time_node_ = std::make_shared(func_name); root_node_ = current_time_node_; } else { current_time_node_ = current_time_node_->add_child(func_name); @@ -122,13 +145,13 @@ void TimeKeeper::end_track(const std::string & func_name) current_time_node_ = current_time_node_->get_parent_node(); } -ScopedStopWatch::ScopedStopWatch(const std::string & func_name, TimeKeeper & time_keepr) +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keepr) : func_name_(func_name), time_keepr_(time_keepr) { time_keepr_.start_track(func_name_); } -ScopedStopWatch::~ScopedStopWatch() +ScopedTimeTrack::~ScopedTimeTrack() { time_keepr_.end_track(func_name_); } diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index e8a3a1589500..8ae6894da8fd 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -6,10 +6,10 @@ // // http://www.apache.org/licenses/LICENSE-2.0 // -// Unless required by applicable law or agreed to in writing, software +// Unlest 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 +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either exprest or implied. +// See the License for the specific language governing permistions and // limitations under the License. #include "autoware/universe_utils/system/time_keeper.hpp" @@ -24,7 +24,7 @@ TEST(system, TimeKeeper) { - using autoware::universe_utils::ScopedStopWatch; + using autoware::universe_utils::ScopedTimeTrack; using autoware::universe_utils::TimeKeeper; rclcpp::Node node{"sample_node"}; @@ -34,15 +34,15 @@ TEST(system, TimeKeeper) time_keeper.start_track("main_func"); { // funcA - ScopedStopWatch ss{"funcA", time_keeper}; + ScopedTimeTrack st{"funcA", time_keeper}; std::this_thread::sleep_for(std::chrono::seconds(1)); } { // funcB - ScopedStopWatch ss{"funcB", time_keeper}; + ScopedTimeTrack st{"funcB", time_keeper}; std::this_thread::sleep_for(std::chrono::seconds(1)); { // funcC - ScopedStopWatch ss{"funcC", time_keeper}; + ScopedTimeTrack st{"funcC", time_keeper}; std::this_thread::sleep_for(std::chrono::seconds(1)); } } diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index 74a3efa17a71..45943d7deec0 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -470,7 +470,7 @@ void MPTOptimizer::onParam(const std::vector & parameters) std::vector MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; const auto & traj_points = p.traj_points; @@ -539,7 +539,7 @@ std::optional> MPTOptimizer::getPrevOptimizedTrajec std::vector MPTOptimizer::calcReferencePoints( const PlannerData & planner_data, const std::vector & smoothed_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; @@ -635,7 +635,7 @@ void MPTOptimizer::updateCurvature( void MPTOptimizer::updateFixedPoint(std::vector & ref_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!prev_ref_points_ptr_) { // no fixed point @@ -691,7 +691,7 @@ void MPTOptimizer::updateDeltaArcLength(std::vector & ref_points void MPTOptimizer::updateExtraPoints(std::vector & ref_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // alpha for (size_t i = 0; i < ref_points.size(); ++i) { @@ -788,7 +788,7 @@ void MPTOptimizer::updateBounds( const std::vector & right_bound, const geometry_msgs::msg::Pose & ego_pose, const double ego_vel) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double soft_road_clearance = mpt_param_.soft_clearance_from_road + vehicle_info_.vehicle_width_m / 2.0; @@ -1094,7 +1094,7 @@ void MPTOptimizer::updateVehicleBounds( std::vector & ref_points, const SplineInterpolationPoints2d & ref_points_spline) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); @@ -1157,7 +1157,7 @@ MPTOptimizer::ValueMatrix MPTOptimizer::calcValueMatrix( const std::vector & ref_points, const std::vector & traj_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1223,7 +1223,7 @@ MPTOptimizer::ObjectiveMatrix MPTOptimizer::calcObjectiveMatrix( [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat, const ValueMatrix & val_mat, const std::vector & ref_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1283,7 +1283,7 @@ MPTOptimizer::ConstraintMatrix MPTOptimizer::calcConstraintMatrix( const StateEquationGenerator::Matrix & mpt_mat, const std::vector & ref_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1462,7 +1462,7 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( const std::vector & ref_points, const ObjectiveMatrix & obj_mat, const ConstraintMatrix & const_mat) { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1629,7 +1629,7 @@ std::optional> MPTOptimizer::calcMPTPoints( std::vector & ref_points, const Eigen::VectorXd & optimized_variables, [[maybe_unused]] const StateEquationGenerator::Matrix & mpt_mat) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = state_equation_generator_.getDimX(); const size_t D_u = state_equation_generator_.getDimU(); @@ -1689,7 +1689,7 @@ void MPTOptimizer::publishDebugTrajectories( const std_msgs::msg::Header & header, const std::vector & ref_points, const std::vector & mpt_traj_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // reference points const auto ref_traj = autoware::motion_utils::convertToTrajectory( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index e15915115589..47aa3914e00c 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -315,7 +315,7 @@ PlannerData PathOptimizer::createPlannerData( std::vector PathOptimizer::generateOptimizedTrajectory( const PlannerData & planner_data) { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & input_traj_points = planner_data.traj_points; @@ -340,7 +340,7 @@ std::vector PathOptimizer::generateOptimizedTrajectory( std::vector PathOptimizer::optimizeTrajectory(const PlannerData & planner_data) { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data; // 1. check if replan (= optimization) is required @@ -385,7 +385,7 @@ void PathOptimizer::applyInputVelocity( const std::vector & input_traj_points, const geometry_msgs::msg::Pose & ego_pose) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // crop forward for faster calculation const auto forward_cropped_input_traj_points = [&]() { @@ -485,7 +485,7 @@ void PathOptimizer::applyInputVelocity( void PathOptimizer::insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & optimized_traj_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (optimized_traj_points.empty()) { return; @@ -549,7 +549,7 @@ void PathOptimizer::insertZeroVelocityOutsideDrivableArea( void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( stop_pose, "outside drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m); @@ -564,7 +564,7 @@ void PathOptimizer::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pos void PathOptimizer::publishDebugMarkerOfOptimization( const std::vector & traj_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (!enable_pub_debug_marker_) { return; @@ -585,7 +585,7 @@ std::vector PathOptimizer::extendTrajectory( const std::vector & traj_points, const std::vector & optimized_traj_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & joint_start_pose = optimized_traj_points.back().pose; @@ -646,7 +646,7 @@ std::vector PathOptimizer::extendTrajectory( void PathOptimizer::publishDebugData(const Header & header) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // publish trajectories const auto debug_extended_traj = diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 5f07cc5ea377..74033c5834db 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -23,7 +23,7 @@ namespace autoware::path_optimizer StateEquationGenerator::Matrix StateEquationGenerator::calcMatrix( const std::vector & ref_points) const { - autoware::universe_utils::ScopedStopWatch ss(__func__, *time_keeper_); + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const size_t D_x = vehicle_model_ptr_->getDimX(); const size_t D_u = vehicle_model_ptr_->getDimU(); From 17e3bdf2ffb94d3e13dd567454d2f39106edb265 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Thu, 4 Jul 2024 16:59:16 +0900 Subject: [PATCH 06/17] fix Signed-off-by: Y.Hisaki --- .../test/src/system/test_time_keeper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index 8ae6894da8fd..f8cddf7f1217 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -6,10 +6,10 @@ // // http://www.apache.org/licenses/LICENSE-2.0 // -// Unlest required by applicable law or agreed to in writing, software +// 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 exprest or implied. -// See the License for the specific language governing permistions and +// 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" From 25c3c3e6ac95fd1b46b7f9ded1b6a02fdb4af63d Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Thu, 4 Jul 2024 17:14:14 +0900 Subject: [PATCH 07/17] fix Signed-off-by: Y.Hisaki --- common/autoware_universe_utils/src/system/time_keeper.cpp | 2 +- planning/autoware_path_optimizer/src/node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 91f24886e8d1..4b61583f7bb7 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -69,7 +69,7 @@ tier4_debug_msgs::msg::ProcessingTimeTree ProcessingTimeNode::to_msg() const tier4_debug_msgs::msg::ProcessingTimeNode time_node_msg; time_node_msg.name = node.name_; time_node_msg.processing_time = node.processing_time_; - time_node_msg.id = tree_msg.nodes.size(); + time_node_msg.id = tree_msg.nodes.size() + 1; time_node_msg.parent_id = parent_id; tree_msg.nodes.emplace_back(time_node_msg); diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 47aa3914e00c..7b720c4cc946 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -277,7 +277,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); time_keeper_->end_track(__func__); - time_keeper_->report(true); + time_keeper_->report(); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const From 0efba7703ec165aa88cca954e196d3a3cca0b719 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Fri, 5 Jul 2024 11:42:45 +0900 Subject: [PATCH 08/17] fix bug of test Signed-off-by: Y.Hisaki --- .../test/src/system/test_time_keeper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index f8cddf7f1217..d66efe5a99ee 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -48,5 +48,5 @@ TEST(system, TimeKeeper) } time_keeper.end_track("main_func"); - ASSERT_ANY_THROW(time_keeper.report(true)); + ASSERT_NO_THROW(time_keeper.report(true)); } From ede0dfe2a85a8db5bf2673e10f223e1561f7a92c Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Fri, 5 Jul 2024 15:25:12 +0900 Subject: [PATCH 09/17] change interface of Timekeeper's constructor and report() Signed-off-by: Y.Hisaki --- .../universe_utils/system/time_keeper.hpp | 45 ++++++++++++++++--- .../src/system/time_keeper.cpp | 23 +--------- .../test/src/system/test_time_keeper.cpp | 8 +++- .../include/autoware/path_optimizer/node.hpp | 4 +- planning/autoware_path_optimizer/src/node.cpp | 7 ++- 5 files changed, 54 insertions(+), 33 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index b06f55c43cbf..4b2a4c2908fa 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -21,7 +21,10 @@ #include #include +#include + #include +#include #include #include @@ -99,6 +102,9 @@ class ProcessingTimeNode : public std::enable_shared_from_this::SharedPtr`, it will publish + * the processing times to the given ROS2 topic. + * + * @tparam Args Types of the arguments + * @param args Variable number of arguments + * @throws std::runtime_error if called before ending the tracking of the current function */ - void report(const bool show_on_terminal = false); + template + void report(Args... args) + { + 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())); + } + + (report_to(args), ...); + current_time_node_.reset(); + root_node_.reset(); + } /** * @brief Start tracking the processing time of a function @@ -134,8 +157,16 @@ class TimeKeeper void end_track(const std::string & func_name); private: - rclcpp::Publisher::SharedPtr - processing_time_pub_; //!< Publisher for the processing time message + void report_to(std::ostream * output) const + { + *output << "========================================" << std::endl; + *output << root_node_->to_string(); + } + void report_to(rclcpp::Publisher::SharedPtr pub) const + { + pub->publish(root_node_->to_msg()); + } + std::shared_ptr current_time_node_; //!< Shared pointer to the current time node std::shared_ptr root_node_; //!< Shared pointer to the root time node diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 4b61583f7bb7..c3446b707b65 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -14,8 +14,6 @@ #include "autoware/universe_utils/system/time_keeper.hpp" -#include - #include #include @@ -99,28 +97,11 @@ std::string ProcessingTimeNode::get_name() const return name_; } -TimeKeeper::TimeKeeper(rclcpp::Node * node) : current_time_node_(nullptr) +TimeKeeper::TimeKeeper() : current_time_node_(nullptr) { - processing_time_pub_ = node->create_publisher( - "~/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_->to_string() << std::endl; - } - - processing_time_pub_->publish(root_node_->to_msg()); - - current_time_node_.reset(); - root_node_.reset(); -} +// void TimeKeeper::report_to void TimeKeeper::start_track(const std::string & func_name) { diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index d66efe5a99ee..faef08423211 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -20,6 +20,7 @@ #include #include +#include #include TEST(system, TimeKeeper) @@ -29,7 +30,10 @@ TEST(system, TimeKeeper) rclcpp::Node node{"sample_node"}; - TimeKeeper time_keeper(&node); + auto publisher = node.create_publisher( + "~/debug/processing_time_tree", 1); + + TimeKeeper time_keeper; time_keeper.start_track("main_func"); @@ -48,5 +52,5 @@ TEST(system, TimeKeeper) } time_keeper.end_track("main_func"); - ASSERT_NO_THROW(time_keeper.report(true)); + ASSERT_NO_THROW(time_keeper.report(&std::cerr, publisher)); } diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp index 0fb92e27bdbf..6f75c649e02b 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp @@ -24,9 +24,9 @@ #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "rclcpp/rclcpp.hpp" #include +#include #include #include @@ -99,6 +99,8 @@ class PathOptimizer : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_markers_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 7b720c4cc946..5ae8ea29042c 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -87,7 +87,7 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), - time_keeper_(std::make_shared(this)) + time_keeper_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -103,6 +103,9 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); debug_calculation_time_float_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); { // parameters // parameter for option @@ -277,7 +280,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); time_keeper_->end_track(__func__); - time_keeper_->report(); + time_keeper_->report(debug_processing_time_detail_pub_); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const From 2ee520eea8eb49488bf78d8891f72038103c22c9 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Sat, 6 Jul 2024 02:25:46 +0900 Subject: [PATCH 10/17] update Signed-off-by: Y.Hisaki --- .../universe_utils/system/time_keeper.hpp | 64 +++++++++---------- .../src/system/time_keeper.cpp | 10 ++- .../test/src/system/test_time_keeper.cpp | 7 +- planning/autoware_path_optimizer/src/node.cpp | 8 ++- 4 files changed, 42 insertions(+), 47 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 4b2a4c2908fa..d78535668e7f 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -111,35 +111,26 @@ using ProcessingTimeDetail = class TimeKeeper { public: - /** - * @brief Construct a new TimeKeeper object - */ - TimeKeeper(); + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } - /** - * @brief Report the processing times and optionally show them on the terminal - * - * This function takes a variable number of arguments. If an argument is of type `std::ostream*`, - * it will output the processing times to the given stream. If an argument is of type - * `rclcpp::Publisher::SharedPtr`, it will publish - * the processing times to the given ROS2 topic. - * - * @tparam Args Types of the arguments - * @param args Variable number of arguments - * @throws std::runtime_error if called before ending the tracking of the current function - */ - template - void report(Args... args) + void add_reporter(std::ostream * os) { - 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())); - } + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); + } - (report_to(args), ...); - current_time_node_.reset(); - root_node_.reset(); + void add_reporter(rclcpp::Publisher::SharedPtr publisher) + { + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); } /** @@ -157,14 +148,18 @@ class TimeKeeper void end_track(const std::string & func_name); private: - void report_to(std::ostream * output) const + void report() { - *output << "========================================" << std::endl; - *output << root_node_->to_string(); - } - void report_to(rclcpp::Publisher::SharedPtr pub) const - { - pub->publish(root_node_->to_msg()); + 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())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); } std::shared_ptr @@ -173,6 +168,9 @@ class TimeKeeper autoware::universe_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times }; /** diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index c3446b707b65..99fdcf559d28 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -97,12 +97,6 @@ std::string ProcessingTimeNode::get_name() const return name_; } -TimeKeeper::TimeKeeper() : current_time_node_(nullptr) -{ -} - -// void TimeKeeper::report_to - void TimeKeeper::start_track(const std::string & func_name) { if (current_time_node_ == nullptr) { @@ -124,6 +118,10 @@ void TimeKeeper::end_track(const std::string & 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(); + + if (current_time_node_ == nullptr) { + report(); + } } ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keepr) diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index faef08423211..71ca7cc74bec 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -33,9 +33,9 @@ TEST(system, TimeKeeper) auto publisher = node.create_publisher( "~/debug/processing_time_tree", 1); - TimeKeeper time_keeper; + TimeKeeper time_keeper(&std::cerr, publisher); - time_keeper.start_track("main_func"); + ScopedTimeTrack st{"main_func", time_keeper}; { // funcA ScopedTimeTrack st{"funcA", time_keeper}; @@ -50,7 +50,4 @@ TEST(system, TimeKeeper) std::this_thread::sleep_for(std::chrono::seconds(1)); } } - - time_keeper.end_track("main_func"); - ASSERT_NO_THROW(time_keeper.report(&std::cerr, publisher)); } diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 5ae8ea29042c..7c73d1ad1fee 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -23,6 +23,7 @@ #include "rclcpp/time.hpp" #include +#include #include namespace autoware::path_optimizer @@ -86,8 +87,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), - debug_data_ptr_(std::make_shared()), - time_keeper_(std::make_shared()) + debug_data_ptr_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/path", 1); @@ -135,6 +135,9 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) traj_param_ = TrajectoryParam(this); } + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); + // create core algorithm pointers with parameter declaration replan_checker_ptr_ = std::make_shared(this, ego_nearest_param_); mpt_optimizer_ptr_ = std::make_shared( @@ -280,7 +283,6 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); time_keeper_->end_track(__func__); - time_keeper_->report(debug_processing_time_detail_pub_); } bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const From 4e7e74366987c01bdaa6984cb4b34ff691d2a7ae Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 13:40:14 +0900 Subject: [PATCH 11/17] add string reporter Signed-off-by: Y.Hisaki --- .../autoware/universe_utils/system/time_keeper.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index d78535668e7f..aaba9be37fc3 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -16,8 +16,10 @@ #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include +#include #include #include @@ -133,6 +135,15 @@ class TimeKeeper }); } + void add_reporter(rclcpp::Publisher::SharedPtr publisher) + { + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + std_msgs::msg::String msg; + msg.data = node->to_string(); + publisher->publish(msg); + }); + } + /** * @brief Start tracking the processing time of a function * From c6792800949cd11137b26b7eb8da142cd607b1b6 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 13:42:41 +0900 Subject: [PATCH 12/17] add manager Signed-off-by: Y.Hisaki --- .../universe_utils/system/time_keeper.hpp | 22 ++++++++++++++++--- .../src/system/time_keeper.cpp | 8 +++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index aaba9be37fc3..881dc702b88a 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -25,6 +25,7 @@ #include +#include #include #include #include @@ -194,9 +195,9 @@ class ScopedTimeTrack * @brief Construct a new ScopedTimeTrack object * * @param func_name Name of the function to be tracked - * @param time_keepr Reference to the TimeKeeper object + * @param time_keeper Reference to the TimeKeeper object */ - ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keepr); + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); /** * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function @@ -205,7 +206,22 @@ class ScopedTimeTrack private: const std::string func_name_; //!< Name of the function being tracked - TimeKeeper & time_keepr_; //!< Reference to the TimeKeeper object + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +class TimeKeeperManager +{ +public: + static std::shared_ptr bring_time_keeper(const std::string & time_keeper_name) + { + if (time_keeper_map_.find(time_keeper_name) == time_keeper_map_.end()) { + time_keeper_map_[time_keeper_name] = std::make_shared(); + } + return time_keeper_map_[time_keeper_name]; + } + +private: + static std::map> time_keeper_map_; }; } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index 99fdcf559d28..c16af346e742 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -124,15 +124,15 @@ void TimeKeeper::end_track(const std::string & func_name) } } -ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keepr) -: func_name_(func_name), time_keepr_(time_keepr) +ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) +: func_name_(func_name), time_keeper_(time_keeper) { - time_keepr_.start_track(func_name_); + time_keeper_.start_track(func_name_); } ScopedTimeTrack::~ScopedTimeTrack() { - time_keepr_.end_track(func_name_); + time_keeper_.end_track(func_name_); } } // namespace autoware::universe_utils From 92c700d505be4ec796d45f3280064ad9c03bd190 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 14:12:17 +0900 Subject: [PATCH 13/17] revert manager Signed-off-by: Y.Hisaki --- .../universe_utils/system/time_keeper.hpp | 28 +++++++++---------- .../test/src/system/test_time_keeper.cpp | 21 ++++++++++++++ 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 881dc702b88a..c19421cd94f2 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -209,20 +209,20 @@ class ScopedTimeTrack TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object }; -class TimeKeeperManager -{ -public: - static std::shared_ptr bring_time_keeper(const std::string & time_keeper_name) - { - if (time_keeper_map_.find(time_keeper_name) == time_keeper_map_.end()) { - time_keeper_map_[time_keeper_name] = std::make_shared(); - } - return time_keeper_map_[time_keeper_name]; - } - -private: - static std::map> time_keeper_map_; -}; +// class TimeKeeperManager +// { +// public: +// static std::shared_ptr bring_time_keeper(const std::string & time_keeper_name) +// { +// if (time_keeper_map_.find(time_keeper_name) == time_keeper_map_.end()) { +// time_keeper_map_[time_keeper_name] = std::make_shared(); +// } +// return time_keeper_map_[time_keeper_name]; +// } + +// private: +// static std::map> time_keeper_map_; +// }; } // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index 71ca7cc74bec..edbbc9598610 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -51,3 +51,24 @@ TEST(system, TimeKeeper) } } } + +// TEST(system, TimeKeeperManager) +// { +// using autoware::universe_utils::ScopedTimeTrack; +// using autoware::universe_utils::TimeKeeperManager; + +// auto time_keepr = TimeKeeperManager::bring_time_keeper("sample"); +// time_keepr->add_reporter(&std::cerr); + +// ScopedTimeTrack st("main_func", *time_keepr); + +// { +// auto time_keepr = TimeKeeperManager::bring_time_keeper("sample"); +// ScopedTimeTrack st("sub_func", *time_keepr); + +// { +// auto time_keepr = TimeKeeperManager::bring_time_keeper("sample"); +// ScopedTimeTrack st("sub_sub_func", *time_keepr); +// } +// } +// } From 47e96688ca1d47a6f9acc40ab32d7ef9bfcc0708 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 14:12:41 +0900 Subject: [PATCH 14/17] remove unused include Signed-off-by: Y.Hisaki --- .../include/autoware/universe_utils/system/time_keeper.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index c19421cd94f2..bdca8e03fe40 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -25,7 +25,6 @@ #include -#include #include #include #include From f7fd72004322657223d61ac15d2856816b2ec6da Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 14:13:13 +0900 Subject: [PATCH 15/17] remove comment Signed-off-by: Y.Hisaki --- .../universe_utils/system/time_keeper.hpp | 15 ------------- .../test/src/system/test_time_keeper.cpp | 21 ------------------- 2 files changed, 36 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index bdca8e03fe40..0b304d63bd6c 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -208,21 +208,6 @@ class ScopedTimeTrack TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object }; -// class TimeKeeperManager -// { -// public: -// static std::shared_ptr bring_time_keeper(const std::string & time_keeper_name) -// { -// if (time_keeper_map_.find(time_keeper_name) == time_keeper_map_.end()) { -// time_keeper_map_[time_keeper_name] = std::make_shared(); -// } -// return time_keeper_map_[time_keeper_name]; -// } - -// private: -// static std::map> time_keeper_map_; -// }; - } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp index edbbc9598610..71ca7cc74bec 100644 --- a/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp +++ b/common/autoware_universe_utils/test/src/system/test_time_keeper.cpp @@ -51,24 +51,3 @@ TEST(system, TimeKeeper) } } } - -// TEST(system, TimeKeeperManager) -// { -// using autoware::universe_utils::ScopedTimeTrack; -// using autoware::universe_utils::TimeKeeperManager; - -// auto time_keepr = TimeKeeperManager::bring_time_keeper("sample"); -// time_keepr->add_reporter(&std::cerr); - -// ScopedTimeTrack st("main_func", *time_keepr); - -// { -// auto time_keepr = TimeKeeperManager::bring_time_keeper("sample"); -// ScopedTimeTrack st("sub_func", *time_keepr); - -// { -// auto time_keepr = TimeKeeperManager::bring_time_keeper("sample"); -// ScopedTimeTrack st("sub_sub_func", *time_keepr); -// } -// } -// } From a4abfc7b676044d6ceb4947a5bdd138db48a9a96 Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 16:22:00 +0900 Subject: [PATCH 16/17] update Signed-off-by: Y.Hisaki --- common/autoware_universe_utils/CMakeLists.txt | 18 ++++ common/autoware_universe_utils/README.md | 91 +++++++++++++++++++ .../examples/example_time_keeper.cpp | 55 +++++++++++ .../universe_utils/system/time_keeper.hpp | 57 +++++------- .../src/system/time_keeper.cpp | 37 ++++++++ 5 files changed, 224 insertions(+), 34 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_time_keeper.cpp diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index f032451db235..2fdeef2119ab 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_universe_utils) +option(BUILD_EXAMPLES "Build examples" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() @@ -37,4 +39,20 @@ if(BUILD_TESTING) ) endif() +if(BUILD_EXAMPLES) + message(STATUS "Building examples") + file(GLOB_RECURSE example_files examples/*.cpp) + + foreach(example_file ${example_files}) + get_filename_component(example_name ${example_file} NAME_WE) + add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} + autoware_universe_utils + ) + install(TARGETS ${example_name} + DESTINATION lib/${PROJECT_NAME} + ) + endforeach() +endif() + ament_auto_package() diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index 22b9355b0963..efcb18844408 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -7,3 +7,94 @@ This package contains many common functions used by other packages, so please re ## For developers `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. + +### TimeKeeper and ScopedTimeTrack + +#### `autoware::universe_utils::TimeKeeper` + +##### Constructor + +```cpp +template +explicit TimeKeeper(Reporters... reporters); +``` + +- Initializes the `TimeKeeper` with a list of reporters. + +##### Methods + +- `void add_reporter(std::ostream * os);` + + - Adds a reporter to output processing times to an `ostream`. + - `os`: Pointer to the `ostream` object. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void add_reporter(rclcpp::Publisher::SharedPtr publisher);` + + - Adds a reporter to publish processing times to an `rclcpp` publisher with `std_msgs::msg::String`. + - `publisher`: Shared pointer to the `rclcpp` publisher. + +- `void start_track(const std::string & func_name);` + + - Starts tracking the processing time of a function. + - `func_name`: Name of the function to be tracked. + +- `void end_track(const std::string & func_name);` + - Ends tracking the processing time of a function. + - `func_name`: Name of the function to end tracking. + +#### Example + +```cpp +#include +#include +#include "autoware_universe_utils/time_keeper.hpp" + + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + time_keeper->add_reporter(&std::cout); + + auto publisher = node->create_publisher("time_topic", 10); + time_keeper->add_reporter(publisher); + + time_keeper->start_track("example_function"); + // Do some processing here + time_keeper->end_track("example_function"); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +#### `autoware::universe_utils::ScopedTimeTrack` + +##### Description + +Class for automatically tracking the processing time of a function within a scope. + +##### Constructor + +```cpp +ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); +``` + +- `func_name`: Name of the function to be tracked. +- `time_keeper`: Reference to the `TimeKeeper` object. + +##### Destructor + +```cpp +~ScopedTimeTrack(); +``` + +- Destroys the `ScopedTimeTrack` object, ending the tracking of the function. diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp new file mode 100644 index 000000000000..cfe8eb089729 --- /dev/null +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -0,0 +1,55 @@ +// 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 + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("time_keeper_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + funcB(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp index 0b304d63bd6c..96070f0f30b7 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/system/time_keeper.hpp @@ -120,29 +120,27 @@ class TimeKeeper (add_reporter(reporters), ...); } - void add_reporter(std::ostream * os) - { - reporters_.emplace_back([os](const std::shared_ptr & node) { - *os << "==========================" << std::endl; - *os << node->to_string() << std::endl; - }); - } + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); - void add_reporter(rclcpp::Publisher::SharedPtr publisher) - { - reporters_.emplace_back([publisher](const std::shared_ptr & node) { - publisher->publish(node->to_msg()); - }); - } + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); - void add_reporter(rclcpp::Publisher::SharedPtr publisher) - { - reporters_.emplace_back([publisher](const std::shared_ptr & node) { - std_msgs::msg::String msg; - msg.data = node->to_string(); - publisher->publish(msg); - }); - } + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher with + * std_msgs::msg::String + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); /** * @brief Start tracking the processing time of a function @@ -159,19 +157,10 @@ class TimeKeeper void end_track(const std::string & func_name); private: - void report() - { - 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())); - } - for (const auto & reporter : reporters_) { - reporter(root_node_); - } - current_time_node_.reset(); - root_node_.reset(); - } + /** + * @brief Report the processing times to all registered reporters + */ + void report(); std::shared_ptr current_time_node_; //!< Shared pointer to the current time node diff --git a/common/autoware_universe_utils/src/system/time_keeper.cpp b/common/autoware_universe_utils/src/system/time_keeper.cpp index c16af346e742..58bed6677227 100644 --- a/common/autoware_universe_utils/src/system/time_keeper.cpp +++ b/common/autoware_universe_utils/src/system/time_keeper.cpp @@ -97,6 +97,30 @@ std::string ProcessingTimeNode::get_name() const return name_; } +void TimeKeeper::add_reporter(std::ostream * os) +{ + reporters_.emplace_back([os](const std::shared_ptr & node) { + *os << "==========================" << std::endl; + *os << node->to_string() << std::endl; + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + publisher->publish(node->to_msg()); + }); +} + +void TimeKeeper::add_reporter(rclcpp::Publisher::SharedPtr publisher) +{ + reporters_.emplace_back([publisher](const std::shared_ptr & node) { + std_msgs::msg::String msg; + msg.data = node->to_string(); + publisher->publish(msg); + }); +} + void TimeKeeper::start_track(const std::string & func_name) { if (current_time_node_ == nullptr) { @@ -124,6 +148,19 @@ void TimeKeeper::end_track(const std::string & func_name) } } +void TimeKeeper::report() +{ + 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())); + } + for (const auto & reporter : reporters_) { + reporter(root_node_); + } + current_time_node_.reset(); + root_node_.reset(); +} + ScopedTimeTrack::ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper) : func_name_(func_name), time_keeper_(time_keeper) { From 6504fa260ee6dbe77f47804f19af0ff299fae8de Mon Sep 17 00:00:00 2001 From: "Y.Hisaki" Date: Mon, 8 Jul 2024 18:38:18 +0900 Subject: [PATCH 17/17] update readme and example Signed-off-by: Y.Hisaki --- common/autoware_universe_utils/README.md | 174 +++++++++++++++++- .../examples/example_scoped_time_track.cpp | 61 ++++++ .../examples/example_time_keeper.cpp | 11 +- 3 files changed, 237 insertions(+), 9 deletions(-) create mode 100644 common/autoware_universe_utils/examples/example_scoped_time_track.cpp diff --git a/common/autoware_universe_utils/README.md b/common/autoware_universe_utils/README.md index efcb18844408..2d24f8442329 100644 --- a/common/autoware_universe_utils/README.md +++ b/common/autoware_universe_utils/README.md @@ -8,7 +8,9 @@ This package contains many common functions used by other packages, so please re `autoware_universe_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. -### TimeKeeper and ScopedTimeTrack +## `autoware::universe_utils` + +### `systems` #### `autoware::universe_utils::TimeKeeper` @@ -47,13 +49,15 @@ explicit TimeKeeper(Reporters... reporters); - Ends tracking the processing time of a function. - `func_name`: Name of the function to end tracking. -#### Example +##### Example ```cpp -#include +#include "autoware/universe_utils/system/time_keeper.hpp" + #include -#include "autoware_universe_utils/time_keeper.hpp" +#include +#include int main(int argc, char ** argv) { @@ -61,14 +65,39 @@ int main(int argc, char ** argv) auto node = std::make_shared("time_keeper_example"); auto time_keeper = std::make_shared(); + time_keeper->add_reporter(&std::cout); - auto publisher = node->create_publisher("time_topic", 10); + auto publisher = + node->create_publisher("processing_time", 10); + time_keeper->add_reporter(publisher); - time_keeper->start_track("example_function"); - // Do some processing here - time_keeper->end_track("example_function"); + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + time_keeper->start_track("funcA"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + time_keeper->end_track("funcA"); + }; + + auto funcB = [&time_keeper, &funcA]() { + time_keeper->start_track("funcB"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + time_keeper->end_track("funcB"); + }; + + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); rclcpp::spin(node); rclcpp::shutdown(); @@ -76,6 +105,44 @@ int main(int argc, char ** argv) } ``` +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` + #### `autoware::universe_utils::ScopedTimeTrack` ##### Description @@ -98,3 +165,94 @@ ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); ``` - Destroys the `ScopedTimeTrack` object, ending the tracking of the function. + +##### Example + +```cpp +#include "autoware/universe_utils/system/time_keeper.hpp" + +#include + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +``` + +- Output (console) + + ```text + ========================== + funcC (6000.7ms) + └── funcB (3000.44ms) + └── funcA (1000.19ms) + ``` + +- Output (`ros2 topic echo /processing_time`) + + ```text + nodes: + - id: 1 + name: funcC + processing_time: 6000.659 + parent_id: 0 + - id: 2 + name: funcB + processing_time: 3000.415 + parent_id: 1 + - id: 3 + name: funcA + processing_time: 1000.181 + parent_id: 2 + --- + ``` + +- Output (`ros2 topic echo /processing_time_str --field data`) + + ```text + funcC (6000.67ms) + └── funcB (3000.42ms) + └── funcA (1000.19ms) + + --- + ``` diff --git a/common/autoware_universe_utils/examples/example_scoped_time_track.cpp b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp new file mode 100644 index 000000000000..010cc58aba8a --- /dev/null +++ b/common/autoware_universe_utils/examples/example_scoped_time_track.cpp @@ -0,0 +1,61 @@ +// 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 + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("scoped_time_track_example"); + + auto time_keeper = std::make_shared(); + + time_keeper->add_reporter(&std::cout); + + auto publisher = + node->create_publisher("processing_time", 10); + + time_keeper->add_reporter(publisher); + + auto publisher_str = node->create_publisher("processing_time_str", 10); + + time_keeper->add_reporter(publisher_str); + + auto funcA = [&time_keeper]() { + autoware::universe_utils::ScopedTimeTrack st("funcA", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(1)); + }; + + auto funcB = [&time_keeper, &funcA]() { + autoware::universe_utils::ScopedTimeTrack st("funcB", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(2)); + funcA(); + }; + + auto funcC = [&time_keeper, &funcB]() { + autoware::universe_utils::ScopedTimeTrack st("funcC", *time_keeper); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + }; + + funcC(); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/autoware_universe_utils/examples/example_time_keeper.cpp b/common/autoware_universe_utils/examples/example_time_keeper.cpp index cfe8eb089729..3f6037e68daa 100644 --- a/common/autoware_universe_utils/examples/example_time_keeper.cpp +++ b/common/autoware_universe_utils/examples/example_time_keeper.cpp @@ -34,6 +34,8 @@ int main(int argc, char ** argv) auto publisher_str = node->create_publisher("processing_time_str", 10); + time_keeper->add_reporter(publisher_str); + auto funcA = [&time_keeper]() { time_keeper->start_track("funcA"); std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -47,7 +49,14 @@ int main(int argc, char ** argv) time_keeper->end_track("funcB"); }; - funcB(); + auto funcC = [&time_keeper, &funcB]() { + time_keeper->start_track("funcC"); + std::this_thread::sleep_for(std::chrono::seconds(3)); + funcB(); + time_keeper->end_track("funcC"); + }; + + funcC(); rclcpp::spin(node); rclcpp::shutdown();