fix(motion_velocity_smoother): fix too slow speed (#308)

Signed-off-by: taikitanaka3 <[email protected]>
tier4 · Mar 16, 2023 · 6582032 · 6582032
1 parent 7047778
commit 6582032
Showing 1 changed file with 7 additions and 5 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
@@ -196,11 +196,13 @@ bool JerkFilteredSmoother::apply(
   }
 
   for (size_t i = 0; i < N; ++i) {
-    const double v_max = std::max(v_max_arr.at(i), 0.1);
-    q.at(IDX_B0 + i) =
-      -1.0 / (v_max * v_max);  // |v_max_i^2 - b_i|/v_max^2 -> minimize (-bi) * ds / v_max^2
-    if (i < N - 1) {
-      q.at(IDX_B0 + i) *= std::max(interval_dist_arr.at(i), 0.0001);
+    if (v_max_arr.at(i) > 0.01) {
+      // Note that if v_max[i] is too small, we did not minimize the corresponding -b[i]
+      double v_weight_term = -1.0 / (v_max_arr.at(i) * v_max_arr.at(i));
+      if (i < N - 1) {
+        v_weight_term *= std::max(interval_dist_arr.at(i), 0.0001);
+      }
+      q.at(IDX_B0 + i) += v_weight_term;
     }
     P(IDX_DELTA0 + i, IDX_DELTA0 + i) += over_v_weight;  // over velocity cost
     P(IDX_SIGMA0 + i, IDX_SIGMA0 + i) += over_a_weight;  // over acceleration cost