diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 1020edea17c13..01f79f2495383 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 retrun path_smoother output."); return smoothed_points; }; @@ -499,8 +504,7 @@ 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"); + RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp"); return get_prev_optimized_traj_points(); } @@ -508,7 +512,7 @@ std::vector MPTOptimizer::optimizeTrajectory( 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