From 7c83e14cb31451fcfe59c3c72f605481f9904fcf Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 17 Feb 2022 11:46:17 +0900 Subject: [PATCH] fix(motion_velocity_smoother): fix bug in merge filter (#393) --- .../src/smoother/jerk_filtered_smoother.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 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 index 7809c7b4828a..9ab84b5ecf95 100644 --- 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; } }