fix(motion_velocity_smoother): fix bug in merge filter (#393)

autowarefoundation · Feb 17, 2022 · ca45e4a · ca45e4a
1 parent 5532d03
commit ca45e4a
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -424,8 +424,7 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory(
   if (getVx(backward_filtered, 0) < v0) {
     double current_vel = v0;
     double current_acc = a0;
-    while (getVx(backward_filtered, i) < current_vel && current_vel <= getVx(forward_filtered, i) &&
-           i < merged.size() - 1) {
+    while (getVx(backward_filtered, i) < current_vel && i < merged.size() - 1) {
       merged.at(i).longitudinal_velocity_mps = current_vel;
       merged.at(i).acceleration_mps2 = current_acc;
 
@@ -443,6 +442,10 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory(
         current_vel = current_vel + current_acc * dt + 0.5 * j_min * dt * dt;
         current_acc = current_acc + j_min * dt;
       }
+
+      if (current_vel > getVx(forward_filtered, i)) {
+        current_vel = getVx(forward_filtered, i);
+      }
       ++i;
     }
   }