fix(motion_velocity_smoother): fix bug in merge filter (autowarefound…

…ation#393)
satoshi-ota · May 31, 2022 · 7c83e14 · 7c83e14
1 parent 740b467
commit 7c83e14
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;
     }
   }