po

Signed-off-by: Yuki Takagi <[email protected]>

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -471,8 +471,13 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
 
   const auto get_prev_optimized_traj_points = [&]() {
     if (prev_optimized_traj_points_ptr_) {
+      RCLCPP_WARN(logger_, "return the previous optimized_trajectory as exceptional behavior.");
       return *prev_optimized_traj_points_ptr_;
     }
+    RCLCPP_WARN(
+      logger_,
+      "Try to return the previous optimized_trajectory as exceptional behavior, "
+      "but this failure also. Then retrun path_smoother output.");
     return smoothed_points;
   };
 
@@ -499,16 +504,15 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
   // 6. optimize steer angles
   const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat);
   if (!optimized_variables) {
-    RCLCPP_INFO_EXPRESSION(
-      logger_, enable_debug_info_, "return std::nullopt since could not solve qp");
+    RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp");
     return get_prev_optimized_traj_points();
   }
 
   // 7. convert to points with validation
   const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat);
   if (!mpt_traj_points) {
     RCLCPP_WARN(logger_, "return std::nullopt since lateral or yaw error is too large.");
-    return get_prev_optimized_traj_points();
+    return smoothed_points;
   }
 
   // 8. publish trajectories for debug