fix(behavior_velocity): fix interpolation add missing last point (#501)

* fix(behavior_velocity): fix interpolation add missing last point

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): add missing last point of clipped path

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): make implementation simpler

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -157,8 +157,9 @@ void clipPathByLength(
   const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length)
 {
   double length_sum = 0;
-  for (int i = 0; i < static_cast<int>(path.points.size()) - 1; i++) {
-    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+  clipped.points.emplace_back(path.points.front());
+  for (int i = 1; i < static_cast<int>(path.points.size()); i++) {
+    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i));
     if (length_sum > max_length) return;
     clipped.points.emplace_back(path.points.at(i));
   }

planning/behavior_velocity_planner/src/utilization/path_utilization.cpp

@@ -106,6 +106,13 @@ bool splineInterpolate(
     output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
     output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
   }
+
+  // insert final point on path if its position is not same point as interpolated
+  const auto & op = output->points.back().point.pose.position;
+  const auto & ip = input.points.back().point.pose.position;
+  if (std::hypot(op.x - ip.x, op.y - ip.y) > ep) {
+    output->points.emplace_back(input.points.back());
+  }
   return true;
 }