chore(behavior_velocity): make implementation simpler

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Mar 10, 2022 · 3bcee0b · 3bcee0b
1 parent ac3c150
commit 3bcee0b
Showing 1 changed file with 3 additions and 5 deletions.
diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
@@ -157,12 +157,10 @@ void clipPathByLength(
   const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length)
 {
   double length_sum = 0;
-  auto p0 = path.points.at(0);
-  for (int i = 0; i < static_cast<int>(path.points.size()); i++) {
-    const auto & p1 = path.points.at(i);
-    length_sum += tier4_autoware_utils::calcDistance2d(p0, p1);
+  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;
-    p0 = p1;
     clipped.points.emplace_back(path.points.at(i));
   }
 }