diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index aae63dc241e80..0039050575734 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -151,8 +152,17 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) debug_pose_publisher_->clearMarkers(); - validate(*current_trajectory_); + static tier4_autoware_utils::StopWatch stopwatch; + static int count = 0; + static double total_ms = 0.0; + stopwatch.tic(); + validate(*current_trajectory_); + const auto t = stopwatch.toc(); + total_ms += t; + count++; + std::cerr << "time = " << t << " [ms] (total " << total_ms << " [ms], count = " << count << ", average_time = " << total_ms / count << ")" << "(traj.size = " << current_trajectory_->points.size() << ")" << std::endl; + diag_updater_.force_update(); publishTrajectory();