fix(motion_velocity_smoother): fix stop point (backport autowarefound…

…ation#1694) (autowarefoundation#567)

fix(motion_velocity_smoother): fix stop point (autowarefoundation#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);