fix(motion_velocity_smoother): fix stop point

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Aug 25, 2022 · 123a555 · 123a555
1 parent 8f7f923
commit 123a555
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -726,7 +726,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);