add stop watch

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

planning/behavior_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_DYNAMIC_AVOIDANCE_MODULE__SCENE_HPP_
 
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
@@ -442,6 +443,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;
 
   TargetObjectsManager target_objects_manager_;
+
+  mutable tier4_autoware_utils::StopWatch<
+    std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
+    stop_watch_;
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_dynamic_avoidance_module/src/scene.cpp

@@ -1845,6 +1845,8 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon(
 std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcFreeRunObstaclePolygon(
   const DynamicAvoidanceObject & object, const PathWithLaneId & ego_path) const
 {
+  stop_watch_.tic(__func__);
+
   constexpr double end_time_to_consider = 3.0;
   constexpr double required_confidence = 0.3;
 
@@ -1919,6 +1921,11 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcFreeR
   tier4_autoware_utils::MultiPolygon2d output_poly;
   boost::geometry::difference(expanded_poly[0], ego_path_restriction_poly, output_poly);
   if (output_poly.empty()) return {};
+
+  const double calculation_time = stop_watch_.toc(__func__);
+  RCLCPP_INFO_EXPRESSION(
+    getLogger(), parameters_->enable_debug_info, "  %s := %f [ms]", __func__, calculation_time);
+
   return output_poly[0];
 }