diff --git a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp
index 66a7dcacbeae1..ee1d48d8522d7 100644
--- a/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp
+++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp
@@ -54,11 +54,11 @@ bool QPSolverOSQP::solve(
   /* execute optimization */
   auto result = osqpsolver_.optimize(h_mat, osqpA, f, lower_bound, upper_bound);
 
-  std::vector<double> U_osqp = std::get<0>(result);
+  std::vector<double> U_osqp = result.primal_solution;
   u = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>>(
     &U_osqp[0], static_cast<Eigen::Index>(U_osqp.size()), 1);
 
-  const int status_val = std::get<3>(result);
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str());
     return false;
@@ -71,7 +71,7 @@ bool QPSolverOSQP::solve(
   }
 
   // polish status: successful (1), unperformed (0), (-1) unsuccessful
-  int status_polish = std::get<2>(result);
+  int status_polish = result.polish_status;
   if (status_polish == -1 || status_polish == 0) {
     const auto s = (status_polish == 0) ? "Polish process is not performed in osqp."
                                         : "Polish process failed in osqp.";
diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp
index 39a35204cda8f..03de35fa0de6f 100644
--- a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp
+++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp
@@ -246,10 +246,11 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   }
 
   // execute optimization
-  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
-  const std::vector<double> optval = std::get<0>(result);
+  const autoware::osqp_interface::OSQPResult result =
+    qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  const std::vector<double> optval = result.primal_solution;
 
-  const int status_val = std::get<3>(result);
+  const int status_val = result.solution_status;
   if (status_val != 1)
     std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl;
 
diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp
index d151fb96d329a..1c395f95a1080 100644
--- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp
+++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp
@@ -21,6 +21,7 @@
 #include "autoware/path_optimizer/common_structs.hpp"
 #include "autoware/path_optimizer/state_equation_generator.hpp"
 #include "autoware/path_optimizer/type_alias.hpp"
+#include "autoware/path_optimizer/utils/conditional_timer.hpp"
 #include "autoware_utils/geometry/geometry.hpp"
 #include "autoware_utils/system/time_keeper.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
@@ -57,6 +58,15 @@ struct Bounds
     lower_bound -= offset;
     upper_bound -= offset;
   }
+
+  friend std::ostream & operator<<(std::ostream & os, const Bounds & bounds)
+  {
+    os << "Bounds: {\n";
+    os << "\tlower_bound: " << bounds.lower_bound << ",\n";
+    os << "\tupper_bound: " << bounds.upper_bound << "\n";
+    os << "}\n";
+    return os;
+  }
 };
 
 struct KinematicState
@@ -65,6 +75,15 @@ struct KinematicState
   double yaw{0.0};
 
   Eigen::Vector2d toEigenVector() const { return Eigen::Vector2d{lat, yaw}; }
+
+  friend std::ostream & operator<<(std::ostream & os, const KinematicState & state)
+  {
+    os << "KinematicState: {\n";
+    os << "\tlat: " << state.lat << ",\n";
+    os << "\tyaw: " << state.yaw << "\n";
+    os << "}\n";
+    return os;
+  }
 };
 
 struct ReferencePoint
@@ -92,6 +111,55 @@ struct ReferencePoint
 
   double getYaw() const { return tf2::getYaw(pose.orientation); }
 
+  friend std::ostream & operator<<(std::ostream & os, const ReferencePoint & ref_point)
+  {
+    os << "ReferencePoint: {\n";
+    os << "\tpose: " << ref_point.pose.position.x << ", " << ref_point.pose.position.y << ",\n";
+    os << "\tlongitudinal_velocity_mps: " << ref_point.longitudinal_velocity_mps << ",\n";
+    os << "\tcurvature: " << ref_point.curvature << ",\n";
+    os << "\tdelta_arc_length: " << ref_point.delta_arc_length << ",\n";
+    os << "\talpha: " << ref_point.alpha << ",\n";
+    os << "\tbounds: " << ref_point.bounds << ",\n";
+    os << "\tbeta: [";
+    for (const auto & b : ref_point.beta) {
+      os << b << ", ";
+    }
+    os << "],\n";
+    os << "\tnormalized_avoidance_cost: " << ref_point.normalized_avoidance_cost << ",\n";
+    os << "\tbounds_on_constraints: [";
+    for (const auto & b : ref_point.bounds_on_constraints) {
+      os << b << ", ";
+    }
+    os << "],\n";
+    os << "\tpose_on_constraints: [";
+    for (const auto & p : ref_point.pose_on_constraints) {
+      os << "(" << p.position.x << ", " << p.position.y << ") , ";
+    }
+    os << "],\n";
+    os << "\tfixed_kinematic_state: ";
+    if (ref_point.fixed_kinematic_state) {
+      os << *ref_point.fixed_kinematic_state;
+    } else {
+      os << "nullopt";
+    }
+    os << ",\n";
+    os << "\toptimized_kinematic_state: " << ref_point.optimized_kinematic_state << ",\n";
+    os << "\toptimized_input: " << ref_point.optimized_input << ",\n";
+    os << "\tslack_variables: ";
+    if (ref_point.slack_variables) {
+      os << "[";
+      for (const auto & s : *ref_point.slack_variables) {
+        os << s << ", ";
+      }
+      os << "]";
+    } else {
+      os << "nullopt";
+    }
+    os << "\n";
+    os << "}\n";
+    return os;
+  }
+
   geometry_msgs::msg::Pose offsetDeviation(const double lat_dev, const double yaw_dev) const
   {
     auto pose_with_deviation = autoware_utils::calc_offset_pose(pose, 0.0, lat_dev, 0.0);
@@ -110,7 +178,7 @@ class MPTOptimizer
     const TrajectoryParam & traj_param, const std::shared_ptr<DebugData> debug_data_ptr,
     const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_);
 
-  std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data);
+  std::optional<std::vector<TrajectoryPoint>> optimizeTrajectory(const PlannerData & planner_data);
   std::optional<std::vector<TrajectoryPoint>> getPrevOptimizedTrajectoryPoints() const;
 
   void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param);
@@ -126,12 +194,30 @@ class MPTOptimizer
   {
     Eigen::SparseMatrix<double> Q;
     Eigen::SparseMatrix<double> R;
+
+    friend std::ostream & operator<<(std::ostream & os, const ValueMatrix & matrix)
+    {
+      os << "ValueMatrix: {\n";
+      os << "\tQ: (Sparse Matrix):" << matrix.Q;
+      os << "\tR: (Sparse Matrix):" << matrix.R;
+      os << "}\n";
+      return os;
+    }
   };
 
   struct ObjectiveMatrix
   {
     Eigen::MatrixXd hessian;
     Eigen::VectorXd gradient;
+
+    friend std::ostream & operator<<(std::ostream & os, const ObjectiveMatrix & matrix)
+    {
+      os << "ObjectiveMatrix: {\n";
+      os << "\thessian:\n" << matrix.hessian << "\n";
+      os << "\tgradient:\n" << matrix.gradient << "\n";
+      os << "}\n";
+      return os;
+    }
   };
 
   struct ConstraintMatrix
@@ -139,6 +225,16 @@ class MPTOptimizer
     Eigen::MatrixXd linear;
     Eigen::VectorXd lower_bound;
     Eigen::VectorXd upper_bound;
+
+    friend std::ostream & operator<<(std::ostream & os, const ConstraintMatrix & matrix)
+    {
+      os << "ConstraintMatrix: {\n";
+      os << "\tlinear:\n" << matrix.linear << "\n";
+      os << "\tlower_bound:\n" << matrix.lower_bound << "\n";
+      os << "\tupper_bound:\n" << matrix.upper_bound << "\n";
+      os << "}\n";
+      return os;
+    }
   };
 
   struct MPTParam
diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp
index 90f8722f1e643..7e77c3bdab97a 100644
--- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp
+++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/node.hpp
@@ -20,6 +20,7 @@
 #include "autoware/path_optimizer/mpt_optimizer.hpp"
 #include "autoware/path_optimizer/replan_checker.hpp"
 #include "autoware/path_optimizer/type_alias.hpp"
+#include "autoware/path_optimizer/utils/conditional_timer.hpp"
 #include "autoware_utils/ros/logger_level_configure.hpp"
 #include "autoware_utils/ros/polling_subscriber.hpp"
 #include "autoware_utils/system/stop_watch.hpp"
@@ -27,6 +28,7 @@
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
 #include <autoware_utils/ros/published_time_publisher.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/publisher.hpp>
 
 #include <algorithm>
@@ -68,6 +70,7 @@ class PathOptimizer : public rclcpp::Node
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_{};
   mutable std::shared_ptr<DebugData> debug_data_ptr_{nullptr};
   mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_{nullptr};
+  mutable std::shared_ptr<ConditionalTimer> conditional_timer_{nullptr};
 
   // flags for some functions
   bool enable_pub_debug_marker_;
@@ -76,6 +79,7 @@ class PathOptimizer : public rclcpp::Node
   bool enable_outside_drivable_area_stop_;
   bool enable_skip_optimization_;
   bool enable_reset_prev_optimization_;
+  bool is_optimization_failed_{false};
   bool use_footprint_polygon_for_outside_drivable_area_check_;
 
   // core algorithms
@@ -123,6 +127,7 @@ class PathOptimizer : public rclcpp::Node
     const std::vector<TrajectoryPoint> & traj_points,
     const std::vector<TrajectoryPoint> & optimized_points) const;
   void publishDebugData(const Header & header) const;
+  void onCheckPathOptimizationValid(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   // functions in generateOptimizedTrajectory
   std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data);
@@ -145,6 +150,9 @@ class PathOptimizer : public rclcpp::Node
   std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
+
+  // diag
+  diagnostic_updater::Updater updater_{this};
 };
 }  // namespace autoware::path_optimizer
 
diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp
index 3c47734fab200..5a74ef447dc14 100644
--- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp
+++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/state_equation_generator.hpp
@@ -37,6 +37,16 @@ class StateEquationGenerator
     Eigen::VectorXd W;
   };
 
+  friend std::ostream & operator<<(std::ostream & os, const Matrix & matrix)
+  {
+    os << "Matrix: {\n";
+    os << "\tA:\n" << matrix.A << "\n";
+    os << "\tB:\n" << matrix.B << "\n";
+    os << "\tW:\n" << matrix.W << "\n";
+    os << "}\n";
+    return os;
+  }
+
   StateEquationGenerator() = default;
   StateEquationGenerator(
     const double wheel_base, const double max_steer_rad,
diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/conditional_timer.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/conditional_timer.hpp
new file mode 100644
index 0000000000000..a218965309d5e
--- /dev/null
+++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/conditional_timer.hpp
@@ -0,0 +1,48 @@
+// Copyright 2025 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <chrono>
+#include <optional>
+
+class ConditionalTimer
+{
+public:
+  void update(bool condition)
+  {
+    if (condition && !start_time_.has_value()) {
+      // Condition met, start the timer
+      start_time_ = std::chrono::high_resolution_clock::now();
+    } else if (!condition && start_time_.has_value()) {
+      // Condition no longer met, stop the timer
+      start_time_ = std::nullopt;
+    }
+  }
+
+  std::chrono::duration<double> getElapsedTime() const
+  {
+    if (start_time_.has_value()) {
+      auto current_time = std::chrono::high_resolution_clock::now();
+      return current_time - *start_time_;
+    } else {
+      return std::chrono::duration<double>(0.0);
+      ;
+    }
+  }
+
+private:
+  std::optional<std::chrono::time_point<std::chrono::high_resolution_clock>> start_time_{
+    std::nullopt};
+};
diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp
index 5a0ba0d5f1128..bb0892767aca8 100644
--- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp
+++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp
@@ -23,6 +23,8 @@
 #include "autoware_utils/math/normalization.hpp"
 #include "tf2/utils.h"
 
+#include <rclcpp/logging.hpp>
+
 #include <algorithm>
 #include <chrono>
 #include <limits>
@@ -471,32 +473,20 @@ void MPTOptimizer::onParam(const std::vector<rclcpp::Parameter> & parameters)
   updateVehicleCircles();
   debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num;
 }
-
-std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data)
+std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::optimizeTrajectory(
+  const PlannerData & planner_data)
 {
   autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   const auto & p = planner_data;
   const auto & traj_points = p.traj_points;
 
-  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 traj_points;
-  };
-
   // 1. calculate reference points
   auto ref_points = calcReferencePoints(planner_data, traj_points);
   if (ref_points.size() < 2) {
     RCLCPP_INFO_EXPRESSION(
       logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2.");
-    return get_prev_optimized_traj_points();
+    return std::nullopt;
   }
 
   // 2. calculate B and W matrices where x = B u + W
@@ -515,14 +505,15 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData
   const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat);
   if (!optimized_variables) {
     RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp");
-    return get_prev_optimized_traj_points();
+
+    return std::nullopt;
   }
 
   // 7. convert to points with validation
-  const auto mpt_traj_points = calcMPTPoints(ref_points, *optimized_variables, mpt_mat);
+  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 std::nullopt;
   }
 
   // 8. publish trajectories for debug
@@ -533,7 +524,7 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData
   prev_optimized_traj_points_ptr_ =
     std::make_shared<std::vector<TrajectoryPoint>>(*mpt_traj_points);
 
-  return *mpt_traj_points;
+  return mpt_traj_points;
 }
 
 std::optional<std::vector<TrajectoryPoint>> MPTOptimizer::getPrevOptimizedTrajectoryPoints() const
@@ -1524,11 +1515,11 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
 
   // solve qp
   time_keeper_->start_track("solveOsqp");
-  const auto result = osqp_solver_ptr_->optimize();
+  const autoware::osqp_interface::OSQPResult osqp_result = osqp_solver_ptr_->optimize();
   time_keeper_->end_track("solveOsqp");
 
   // check solution status
-  const int solution_status = std::get<3>(result);
+  const int solution_status = osqp_result.solution_status;
   prev_solution_status_ = solution_status;
   if (solution_status != 1) {
     osqp_solver_ptr_->logUnsolvedStatus("[MPT]");
@@ -1536,17 +1527,17 @@ std::optional<Eigen::VectorXd> MPTOptimizer::calcOptimizedSteerAngles(
   }
 
   // print iteration
-  const int iteration_status = std::get<4>(result);
+  const int iteration_status = osqp_result.iteration_status;
   RCLCPP_INFO_EXPRESSION(logger_, enable_debug_info_, "iteration: %d", iteration_status);
 
   // get optimization result
   auto optimization_result =
-    std::get<0>(result);  // NOTE: const cannot be added due to the next operation.
+    osqp_result.primal_solution;  // NOTE: const cannot be added due to the next operation.
+
   const auto has_nan = std::any_of(
     optimization_result.begin(), optimization_result.end(),
     [](const auto v) { return std::isnan(v); });
   if (has_nan) {
-    RCLCPP_WARN(logger_, "optimization failed: result contains NaN values");
     return std::nullopt;
   }
   const Eigen::VectorXd optimized_variables =
diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp
index 1d69afd057f4f..f8a4571464d7b 100644
--- a/planning/autoware_path_optimizer/src/node.cpp
+++ b/planning/autoware_path_optimizer/src/node.cpp
@@ -27,6 +27,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::path_optimizer
@@ -89,7 +90,8 @@ std::vector<double> calcSegmentLengthVector(const std::vector<TrajectoryPoint> &
 PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options)
 : Node("path_optimizer", node_options),
   vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()),
-  debug_data_ptr_(std::make_shared<DebugData>())
+  debug_data_ptr_(std::make_shared<DebugData>()),
+  conditional_timer_(std::make_shared<ConditionalTimer>())
 {
   // interface publisher
   traj_pub_ = create_publisher<Trajectory>("~/output/path", 1);
@@ -134,6 +136,13 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options)
 
     // parameters for trajectory
     traj_param_ = TrajectoryParam(this);
+
+    // Diagnostics
+    {
+      updater_.setHardwareID("path_optimizer");
+      updater_.add(
+        "path_optimizer_emergency_stop", this, &PathOptimizer::onCheckPathOptimizationValid);
+    }
   }
 
   time_keeper_ = std::make_shared<autoware_utils::TimeKeeper>(debug_processing_time_detail_pub_);
@@ -267,6 +276,7 @@ void PathOptimizer::onPath(const Path::ConstSharedPtr path_ptr)
 
   // 5. publish debug data
   publishDebugData(planner_data.header);
+  updater_.force_update();
 
   // publish calculation_time
   // NOTE: This function must be called after measuring onPath calculation time
@@ -294,6 +304,20 @@ bool PathOptimizer::checkInputPath(const Path & path, rclcpp::Clock clock) const
   return true;
 }
 
+void PathOptimizer::onCheckPathOptimizationValid(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  if (is_optimization_failed_) {
+    const std::string error_msg =
+      "[Path Optimizer]: Emergency Brake due to prolonged MPT Optimizer failure";
+    const auto diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.summary(diag_level, error_msg);
+  } else {
+    const std::string error_msg = "[Path Optimizer]: MPT Optimizer successful";
+    const auto diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+    stat.summary(diag_level, error_msg);
+  }
+}
+
 PlannerData PathOptimizer::createPlannerData(
   const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const
 {
@@ -315,22 +339,15 @@ std::vector<TrajectoryPoint> PathOptimizer::generateOptimizedTrajectory(
 {
   autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
-  const auto & input_traj_points = planner_data.traj_points;
-
   // 1. calculate trajectory with MPT
   //    NOTE: This function may return previously optimized trajectory points.
   //          Also, velocity on some points will not be updated for a logic purpose.
   auto optimized_traj_points = optimizeTrajectory(planner_data);
 
-  // 2. update velocity
-  //    NOTE: When optimization failed or is skipped, velocity in trajectory points must
-  //          be updated since velocity in input trajectory (path) may change.
-  applyInputVelocity(optimized_traj_points, input_traj_points, planner_data.ego_pose);
-
-  // 3. insert zero velocity when trajectory is over drivable area
+  // 2. insert zero velocity when trajectory is over drivable area
   insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points);
 
-  // 4. publish debug marker
+  // 3. publish debug marker
   publishDebugMarkerOfOptimization(optimized_traj_points);
 
   return optimized_traj_points;
@@ -339,6 +356,7 @@ std::vector<TrajectoryPoint> PathOptimizer::generateOptimizedTrajectory(
 std::vector<TrajectoryPoint> PathOptimizer::optimizeTrajectory(const PlannerData & planner_data)
 {
   autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  is_optimization_failed_ = false;
   const auto & p = planner_data;
 
   // 1. check if replan (= optimization) is required
@@ -365,7 +383,23 @@ std::vector<TrajectoryPoint> PathOptimizer::optimizeTrajectory(const PlannerData
   //    with model predictive trajectory
   const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data);
 
-  return mpt_traj;
+  const bool optimized_traj_failed = static_cast<bool>(mpt_traj);
+
+  conditional_timer_->update(optimized_traj_failed);
+
+  const double elapsed_time = conditional_timer_->getElapsedTime().count();
+  const bool elapsed_time_over_three_seconds = (elapsed_time > 3.0);
+
+  auto optimized_traj_points =
+    optimized_traj_failed && elapsed_time_over_three_seconds ? p.traj_points : std::move(*mpt_traj);
+  is_optimization_failed_ = optimized_traj_failed && elapsed_time_over_three_seconds;
+
+  // 3. update velocity
+  //    NOTE: When optimization failed or is skipped, velocity in trajectory points must
+  //          be updated since velocity in input trajectory (path) may change.
+  applyInputVelocity(optimized_traj_points, p.traj_points, planner_data.ego_pose);
+
+  return optimized_traj_points;
 }
 
 std::vector<TrajectoryPoint> PathOptimizer::getPrevOptimizedTrajectory(
diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp
index 59cc4024aeedf..f4532e429dbdc 100644
--- a/planning/autoware_path_smoother/src/elastic_band.cpp
+++ b/planning/autoware_path_smoother/src/elastic_band.cpp
@@ -403,9 +403,9 @@ std::optional<std::vector<double>> EBPathSmoother::calcSmoothedTrajectory()
 
   // solve QP
   const auto result = osqp_solver_ptr_->optimize();
-  const auto optimized_points = std::get<0>(result);
+  const auto optimized_points = result.primal_solution;
 
-  const auto status = std::get<3>(result);
+  const auto status = result.solution_status;
 
   // check status
   if (status != 1) {
diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
index 1c031e97417a6..66d09c518f394 100644
--- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -193,8 +193,8 @@ bool L2PseudoJerkSmoother::apply(
 
   // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
   //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
+  const std::vector<double> optval = result.primal_solution;
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
     return false;
diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
index a148f92d72b89..6b21a59455955 100644
--- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
+++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -208,8 +208,8 @@ bool LinfPseudoJerkSmoother::apply(
 
   // [b0, b1, ..., bN, |  a0, a1, ..., aN, |
   //  delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN]
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
+  const std::vector<double> optval = result.primal_solution;
+  const int status_val = result.solution_status;
   if (status_val != 1) {
     RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
     return false;
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp
index d8e8c1aff5474..2f6dcbe4f1ec6 100644
--- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp
+++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/optimization_based_planner/velocity_optimizer.cpp
@@ -248,10 +248,11 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza
   }
 
   // execute optimization
-  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
-  const std::vector<double> optval = std::get<0>(result);
+  const autoware::osqp_interface::OSQPResult result =
+    qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  const std::vector<double> optval = result.primal_solution;
 
-  const int status_val = std::get<3>(result);
+  const int status_val = result.solution_status;
   if (status_val != 1)
     std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl;
 
diff --git a/system/autoware_system_diagnostic_monitor/config/planning.yaml b/system/autoware_system_diagnostic_monitor/config/planning.yaml
index c403fec2371c3..6f4e72be94fac 100644
--- a/system/autoware_system_diagnostic_monitor/config/planning.yaml
+++ b/system/autoware_system_diagnostic_monitor/config/planning.yaml
@@ -9,6 +9,7 @@ units:
           - { type: link, link: /autoware/planning/topic_rate_check/route }
           - { type: link, link: /autoware/planning/topic_rate_check/trajectory }
           - { type: link, link: /autoware/planning/trajectory_validation }
+          - { type: link, link: /autoware/planning/emergency_stop }
 
   - path: /autoware/planning/trajectory_validation
     type: and
@@ -24,6 +25,11 @@ units:
       - { type: link, link: /autoware/planning/trajectory_validation/steering_rate }
       - { type: link, link: /autoware/planning/trajectory_validation/velocity_deviation }
 
+  - path: /autoware/planning/emergency_stop
+    type: and
+    list:
+      - { type: link, link: /planning/path_optimizer_emergency_stop-error }
+
   - path: /autoware/planning/routing/state
     type: diag
     node: component_state_diagnostics
@@ -88,3 +94,15 @@ units:
     type: diag
     node: planning_validator
     name: trajectory_validation_velocity_deviation
+
+  - path: /planning/path_optimizer_emergency_stop-error
+    type: warn-to-ok
+    item:
+      type: link
+      link: /planning/path_optimizer_emergency_stop
+
+  - path: /planning/path_optimizer_emergency_stop
+    type: diag
+    node: path_optimizer
+    name: path_optimizer_emergency_stop
+    timeout: 1.0