fix

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

planning/obstacle_cruise_planner/src/planner_interface.cpp

@@ -388,9 +388,12 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));
 
     // Publish if ego vehicle will over run against the stop point with a limit acceleration
-    const bool will_over_run = determined_stop_obstacle->dist_to_collide_on_decimated_traj -
-                                 determined_zero_vel_dist.value() <
-                               determined_desired_margin.value();
+
+    const bool will_over_run = determined_zero_vel_dist.value() >
+                               motion_utils::calcSignedArcLength(
+                                 planner_data.traj_points, 0, planner_data.ego_pose.position) +
+                                 determined_stop_obstacle->dist_to_collide_on_decimated_traj +
+                                 determined_desired_margin.value();
     const auto stop_speed_exceeded_msg =
       createStopSpeedExceededMsg(planner_data.current_time, will_over_run);
     stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);