fix: interpolation (#791) (#218)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

@@ -23,6 +23,8 @@
 #include <memory>
 #include <vector>
 
+constexpr double DOUBLE_EPSILON = 1e-6;
+
 namespace behavior_velocity_planner
 {
 autoware_auto_planning_msgs::msg::Path interpolatePath(
@@ -31,10 +33,11 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
   const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")};
   autoware_auto_planning_msgs::msg::Path interpolated_path;
 
-  std::vector<double> x, x_interp;
-  std::vector<double> y, y_interp;
-  std::vector<double> z, z_interp;
-  std::vector<double> v, v_interp;
+  const double epsilon = 0.01;
+  std::vector<double> x;
+  std::vector<double> y;
+  std::vector<double> z;
+  std::vector<double> v;
   std::vector<double> s_in, s_out;
   if (2000 < path.points.size()) {
     RCLCPP_WARN(
@@ -46,7 +49,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
     return path;
   }
 
-  // Calculate sample points
+  double path_len = length;
   {
     double s = 0.0;
     for (size_t idx = 0; idx < path.points.size(); ++idx) {
@@ -59,28 +62,36 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
         const auto path_point_prev = path.points.at(idx - 1);
         s += tier4_autoware_utils::calcDistance3d(path_point_prev.pose, path_point.pose);
       }
+      if (s > path_len) {
+        break;
+      }
       s_in.push_back(s);
     }
+
+    // update path length
+    path_len = std::min(path_len, s_in.back());
+
+    // Check Terminal Points
+    if (std::fabs(s_in.back() - path_len) < epsilon) {
+      s_in.back() = path_len;
+    } else {
+      s_in.push_back(path_len);
+    }
   }
 
   // Calculate query points
   // Remove query point if query point is near input path point
-  const double epsilon = 0.01;
-  const double interpolation_interval = interval;
-  size_t checkpoint_idx = 1;
-  for (double s = interpolation_interval; s < std::min(length, s_in.back());
-       s += interpolation_interval) {
-    while (checkpoint_idx < s_in.size() && s_in.at(checkpoint_idx) < s) {
-      s_out.push_back(s_in.at(checkpoint_idx));
-      v_interp.push_back(v.at(checkpoint_idx));
-      ++checkpoint_idx;
-    }
-    if (
-      std::fabs(s - s_in.at(checkpoint_idx - 1)) > epsilon &&
-      std::fabs(s - s_in.at(checkpoint_idx)) > epsilon) {
-      s_out.push_back(s);
-      v_interp.push_back(v.at(checkpoint_idx - 1));
+  std::vector<double> s_tmp = s_in;
+  for (double s = 0.0; s < path_len; s += interval) {
+    s_tmp.push_back(s);
+  }
+  std::sort(s_tmp.begin(), s_tmp.end());
+
+  for (const double s : s_tmp) {
+    if (!s_out.empty() && std::fabs(s_out.back() - s) < epsilon) {
+      continue;
     }
+    s_out.push_back(s);
   }
 
   if (s_out.empty()) {
@@ -89,19 +100,21 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
   }
 
   // Interpolate
-  x_interp = interpolation::slerp(s_in, x, s_out);
-  y_interp = interpolation::slerp(s_in, y, s_out);
-  z_interp = interpolation::slerp(s_in, z, s_out);
-
-  // Insert boundary points
-  x_interp.insert(x_interp.begin(), x.front());
-  y_interp.insert(y_interp.begin(), y.front());
-  z_interp.insert(z_interp.begin(), z.front());
-  v_interp.insert(v_interp.begin(), v.front());
-
-  x_interp.push_back(x.back());
-  y_interp.push_back(y.back());
-  z_interp.push_back(z.back());
+  const auto x_interp = interpolation::slerp(s_in, x, s_out);
+  const auto y_interp = interpolation::slerp(s_in, y, s_out);
+  const auto z_interp = interpolation::slerp(s_in, z, s_out);
+
+  std::vector<double> v_interp;
+  size_t closest_segment_idx = 0;
+  for (size_t i = 0; i < s_out.size() - 1; ++i) {
+    for (size_t j = closest_segment_idx; j < s_in.size() - 1; ++j) {
+      if (s_in.at(j) < s_out.at(i) + DOUBLE_EPSILON && s_out.at(i) < s_in.at(j + 1)) {
+        // find closest segment in s_in
+        closest_segment_idx = j;
+      }
+    }
+    v_interp.push_back(v.at(closest_segment_idx));
+  }
   v_interp.push_back(v.back());
 
   // Insert path point to interpolated_path