diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
index fd345df707767..70b6eef958694 100644
--- a/planning/obstacle_stop_planner/src/node.cpp
+++ b/planning/obstacle_stop_planner/src/node.cpp
@@ -866,6 +866,10 @@ void ObstacleStopPlannerNode::insertVelocity(
     }
   } else if (!no_hunting_collision_point) {
     if (latest_stop_point_) {
+      // update stop point index with the current trajectory
+      latest_stop_point_.get().index = findFirstNearestSegmentIndexWithSoftConstraints(
+        output, getPose(latest_stop_point_.get().point), node_param_.ego_nearest_dist_threshold,
+        node_param_.ego_nearest_yaw_threshold);
       insertStopPoint(latest_stop_point_.get(), output, planner_data.stop_reason_diag);
       debug_ptr_->pushPose(getPose(latest_stop_point_.get().point), PoseType::Stop);
     }