fix

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

Author: PanConChicharron

planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp

@@ -323,6 +323,7 @@ class MPTOptimizer
   mutable std::shared_ptr<DebugData> debug_data_ptr_;
   mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_;
   rclcpp::Logger logger_;
+  mutable std::shared_ptr<ConditionalTimer> conditional_timer_{nullptr};
   MPTParam mpt_param_;
 
   StateEquationGenerator state_equation_generator_;

planning/autoware_path_optimizer/src/mpt_optimizer.cpp

@@ -420,7 +420,8 @@ MPTOptimizer::MPTOptimizer(
   traj_param_(traj_param),
   debug_data_ptr_(debug_data_ptr),
   time_keeper_(time_keeper),
-  logger_(node->get_logger().get_child("mpt_optimizer"))
+  logger_(node->get_logger().get_child("mpt_optimizer")),
+  conditional_timer_(std::make_shared<ConditionalTimer>())
 {
   // initialize mpt param
   mpt_param_ = MPTParam(node, vehicle_info);
@@ -504,6 +505,17 @@ std::pair<std::vector<TrajectoryPoint>, int> MPTOptimizer::optimizeTrajectory(
       "but this failure also. Then return path_smoother output.");
     return traj_points;
   };
+  
+  conditional_timer_->update(true);
+
+  const double elapsed_time = conditional_timer_->getElapsedTime().count();
+  const bool elapsed_time_over_ten_seconds = (elapsed_time > 20.0);
+
+  std::cout << "MPT elapsed time: " << elapsed_time << "\n";
+
+  if (elapsed_time_over_ten_seconds) {
+    return std::make_pair(get_prev_optimized_traj_points(), -1);
+  }
 
   // 1. calculate reference points
   auto ref_points = calcReferencePoints(planner_data, traj_points);