clean up

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

Author: PanConChicharron

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

@@ -321,6 +321,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

@@ -128,18 +128,6 @@ std::vector<double> toStdVector(const Eigen::VectorXd & eigen_vec)
   return {eigen_vec.data(), eigen_vec.data() + eigen_vec.rows()};
 }
 
-std::string vectorToString(const std::vector<double> & vec)
-{
-  std::ostringstream oss;
-  if (!vec.empty()) {
-    oss << vec.front();  // Add the first element
-    for (size_t i = 1; i < vec.size(); ++i) {
-      oss << "," << vec[i];  // Add the rest with commas
-    }
-  }
-  return oss.str();
-}
-
 bool isLeft(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_pos)
 {
   const double base_theta = tf2::getYaw(pose.orientation);
@@ -420,7 +408,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);
@@ -505,6 +494,15 @@ std::pair<std::vector<TrajectoryPoint>, int> MPTOptimizer::optimizeTrajectory(
     return traj_points;
   };
 
+  conditional_timer_->update(true);
+
+  const double elapsed_time = conditional_timer_->getElapsedTime().count();
+  const bool elapsed_time_over_twenty_seconds = (elapsed_time > 20.0);
+
+  if (elapsed_time_over_twenty_seconds) {
+    return std::make_pair(get_prev_optimized_traj_points(), -1);
+  }
+
   // 1. calculate reference points
   auto ref_points = calcReferencePoints(planner_data, traj_points);
   if (ref_points.size() < 2) {
@@ -1546,10 +1544,6 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
   const int solution_status = osqp_result.solution_status;
   prev_solution_status_ = solution_status;
   if (solution_status != 1) {
-    const std::vector<double> lagrange_multiplier = osqp_result.lagrange_multipliers;
-    const int status_polish = osqp_result.polish_status;
-    const int exit_flag = osqp_result.exit_flag;
-
     osqp_solver_ptr_->logUnsolvedStatus("[MPT]");
     return std::nullopt;
   }
@@ -1566,10 +1560,6 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
     optimization_result.begin(), optimization_result.end(),
     [](const auto v) { return std::isnan(v); });
   if (has_nan) {
-    const std::vector<double> lagrange_multiplier = osqp_result.lagrange_multipliers;
-    const int status_polish = osqp_result.polish_status;
-    const int exit_flag = osqp_result.exit_flag;
-
     return std::nullopt;
   }
   const Eigen::VectorXd optimized_variables =