fix(path_optimizer): revert the feature of publishing processing time

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

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/stop_watch.hpp"
 #include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
@@ -143,6 +144,8 @@ class PathOptimizer : public rclcpp::Node
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
+
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
 };
 }  // namespace autoware::path_optimizer
 

planning/autoware_path_optimizer/src/node.cpp

@@ -223,6 +223,7 @@ void PathOptimizer::resetPreviousData()
 void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr)
 {
   time_keeper_->start_track(__func__);
+  stop_watch_.tic();
 
   // check if input path is valid
   if (!checkInputPath(*path_ptr, *get_clock())) {
@@ -270,12 +271,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr)
 
   // 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()));
-  */
+  debug_calculation_time_float_pub_->publish(createFloat64Stamped(now(), stop_watch_.toc()));
 
   const auto output_traj_msg =
     autoware::motion_utils::convertToTrajectory(full_traj_points, path_ptr->header);