diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 1020edea17c13..19c74024596c5 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -471,8 +471,13 @@ std::vector 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 return path_smoother output."); return smoothed_points; }; @@ -499,9 +504,8 @@ std::vector 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"); - return get_prev_optimized_traj_points(); + RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp"); + return smoothed_points; } // 7. convert to points with validation