diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
index 6a66b485d1c59..1d4cb43b15978 100644
--- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
+++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
@@ -243,6 +243,10 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void insertYieldVelocity(ShiftedPath & shifted_path) const;
 
+  void insertDecelProfile(
+    PathWithLaneId & path, const double shift_length, const double to_stop_line,
+    const double lower_speed) const;
+
   /**
    * @brief calculate stop distance based on object's overhang.
    * @param stop distance.
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
index d02abbb2d2bd8..afb32133b67ed 100644
--- a/planning/behavior_path_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -1513,33 +1513,37 @@ void AvoidanceModule::insertReturnDeadLine(
   utils::avoidance::insertDecelPoint(
     getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_);
 
-  // insert slow down speed.
-  const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
-    shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
-  if (current_target_velocity < getEgoSpeed()) {
-    RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed.");
-    return;
-  }
-
-  const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
-  for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
-    const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
-
-    // slow down speed is inserted only in front of the object.
-    const auto shift_longitudinal_distance = to_stop_line - distance_from_ego;
-    if (shift_longitudinal_distance < 0.0) {
-      break;
-    }
-
-    // target speed with nominal jerk limits.
-    const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
-      shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance);
-    const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
-    const double v_insert =
-      std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed);
-
-    shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
-  }
+  // // insert slow down speed.
+  // const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
+  //   shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
+  // if (current_target_velocity < getEgoSpeed()) {
+  //   RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed.");
+  //   return;
+  // }
+
+  insertDecelProfile(
+    shifted_path.path, shift_length, to_stop_line, parameters_->min_slow_down_speed);
+
+  // const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
+  // for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
+  //   const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
+
+  //   // slow down speed is inserted only in front of the object.
+  //   const auto shift_longitudinal_distance = to_stop_line - distance_from_ego;
+  //   if (shift_longitudinal_distance < 0.0) {
+  //     break;
+  //   }
+
+  //   // target speed with nominal jerk limits.
+  //   const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
+  //     shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance);
+  //   const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
+  //   const double v_insert =
+  //     std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed);
+
+  //   shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original,
+  //   v_insert);
+  // }
 }
 
 void AvoidanceModule::insertWaitPoint(
@@ -1691,45 +1695,96 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
   const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length);
   const auto distance_to_object = object.longitudinal;
   const auto remaining_distance = distance_to_object - min_avoid_distance;
-  const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front());
-  if (remaining_distance < decel_distance) {
+  const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed;
+  // const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed);
+  // if (remaining_distance < decel_distance) {
+  //   return;
+  // }
+  if (distance_to_object < min_avoid_distance) {
+    RCLCPP_DEBUG(getLogger(), "less than minimum avoidance distance.");
     return;
   }
 
-  // decide slow down lower limit.
-  const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed;
+  // If the stop distance is not enough for comfortable stop, don't insert wait point.
+  const auto is_comfortable_stop =
+    helper_->getFeasibleDecelDistance(lower_speed) < remaining_distance;
+  if (!is_comfortable_stop) {
+    RCLCPP_DEBUG(getLogger(), "stop distance is not enough.");
+    return;
+  }
+
+  // // insert slow down speed.
+  // constexpr double BUFFER = 1.0;  // [m/s]
+  // const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
+  //   shift_length, helper_->getLateralMinJerkLimit(), distance_to_object);
+  // if (current_target_velocity + BUFFER < getEgoSpeed()) {
+  //   utils::avoidance::insertDecelPoint(
+  //     getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path,
+  //     slow_pose_);
+  //   return;
+  // }
+
+  insertDecelProfile(shifted_path.path, shift_length, distance_to_object, lower_speed);
+
+  // const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
+  // for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
+  //   const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
+
+  //   // slow down speed is inserted only in front of the object.
+  //   const auto shift_longitudinal_distance = distance_to_object - distance_from_ego;
+  //   if (shift_longitudinal_distance < min_avoid_distance) {
+  //     break;
+  //   }
+
+  //   // target speed with nominal jerk limits.
+  //   const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
+  //     shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance);
+  //   const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
+  //   const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed);
+
+  //   shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original,
+  //   v_insert);
+  // }
+
+  // slow_pose_ = motion_utils::calcLongitudinalOffsetPose(
+  //   shifted_path.path.points, start_idx, distance_to_object);
+}
+
+void AvoidanceModule::insertDecelProfile(
+  PathWithLaneId & path, const double shift_length, const double to_stop_line,
+  const double lower_speed) const
+{
+  const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed);
 
   // insert slow down speed.
   const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(
-    shift_length, helper_->getLateralMinJerkLimit(), distance_to_object);
+    shift_length, helper_->getLateralMinJerkLimit(), to_stop_line);
   if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) {
     utils::avoidance::insertDecelPoint(
-      getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path,
-      slow_pose_);
+      getEgoPosition(), decel_distance, lower_speed, path, slow_pose_);
     return;
   }
 
-  const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
-  for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
-    const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
+  const auto start_idx = planner_data_->findEgoIndex(path.points);
+  for (size_t i = start_idx; i < path.points.size(); ++i) {
+    const auto distance_from_ego = calcSignedArcLength(path.points, start_idx, i);
 
     // slow down speed is inserted only in front of the object.
-    const auto shift_longitudinal_distance = distance_to_object - distance_from_ego;
-    if (shift_longitudinal_distance < min_avoid_distance) {
+    const auto shift_longitudinal_distance = to_stop_line - distance_from_ego;
+    if (shift_longitudinal_distance < 0.0) {
       break;
     }
 
     // target speed with nominal jerk limits.
     const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
       shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance);
-    const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
+    const double v_original = path.points.at(i).point.longitudinal_velocity_mps;
     const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed);
 
-    shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
+    path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
   }
 
-  slow_pose_ = motion_utils::calcLongitudinalOffsetPose(
-    shifted_path.path.points, start_idx, distance_to_object);
+  slow_pose_ = motion_utils::calcLongitudinalOffsetPose(path.points, start_idx, to_stop_line);
 }
 
 std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const