feat(motion_velocity_smoother): add guard before interpolation (#1752)

* feat(motion_velocity_smoother): add guard before interpolation

Signed-off-by: yutaka <purewater0901@gmail.com>

* udpate

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp

@@ -234,6 +234,18 @@ bool calcStopVelocityWithConstantJerkAccLimit(
     dists.push_back(dist);
   }
 
+  if (
+    !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(dists) ||
+    !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(dists)) {
+    return false;
+  }
+
+  if (
+    xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || dists.empty() ||
+    dists.front() < xs.front() || xs.back() < dists.back()) {
+    return false;
+  }
+
   const auto vel_at_wp = interpolation::lerp(xs, vs, dists);
   const auto acc_at_wp = interpolation::lerp(xs, as, dists);
   const auto jerk_at_wp = interpolation::lerp(xs, js, dists);

planning/motion_velocity_smoother/src/trajectory_utils.cpp

@@ -495,6 +495,18 @@ boost::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
     distance_all.begin(), distance_all.end(), [&xs](double x) { return x > xs.back(); });
   const std::vector<double> distance{distance_all.begin() + start_index, it_end};
 
+  if (
+    !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distance) ||
+    !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distance)) {
+    return {};
+  }
+
+  if (
+    xs.size() < 2 || vs.size() < 2 || as.size() < 2 || distance.empty() ||
+    distance.front() < xs.front() || xs.back() < distance.back()) {
+    return {};
+  }
+
   const auto vel_at_wp = interpolation::lerp(xs, vs, distance);
   const auto acc_at_wp = interpolation::lerp(xs, as, distance);