fix(motion_velocity_smoother): fix stop point (backport #1694) (#567)

fix(motion_velocity_smoother): fix stop point (#1694)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -732,7 +732,7 @@ void MotionVelocitySmootherNode::overwriteStopPoint(
     double optimized_stop_point_vel =
       output.at(*nearest_output_point_idx).longitudinal_velocity_mps;
     is_stop_velocity_exceeded = (optimized_stop_point_vel > over_stop_velocity_warn_thr_);
-    input_stop_vel = input.at(*nearest_output_point_idx).longitudinal_velocity_mps;
+    input_stop_vel = input.at(*stop_idx).longitudinal_velocity_mps;
     output_stop_vel = output.at(*nearest_output_point_idx).longitudinal_velocity_mps;
     trajectory_utils::applyMaximumVelocityLimit(
       *nearest_output_point_idx, output.size(), 0.0, output);