fix(motion_velocity_smoother): fix overwriteStopPoint using backward …

…point (tier4#816) * fix(motion_velocity_smoother): fix overwriteStopPoint using backward point Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Modify overwriteStopPoint input and output Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
boyali · Oct 3, 2022 · 16b527f · 16b527f
1 parent 870240f
commit 16b527f
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -545,13 +545,13 @@ bool MotionVelocitySmootherNode::smoothVelocity(
     RCLCPP_WARN(get_logger(), "Fail to solve optimization.");
   }
 
+  // Set 0 velocity after input-stop-point
+  overwriteStopPoint(clipped, traj_smoothed);
+
   traj_smoothed.insert(
     traj_smoothed.begin(), traj_resampled->begin(),
     traj_resampled->begin() + *traj_resampled_closest);
 
-  // Set 0 velocity after input-stop-point
-  overwriteStopPoint(*traj_resampled, traj_smoothed);
-
   // For the endpoint of the trajectory
   if (!traj_smoothed.empty()) {
     traj_smoothed.back().longitudinal_velocity_mps = 0.0;