fix path bound cut issue

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
autowarefoundation · Feb 18, 2025 · 6e1d40b · 6e1d40b
1 parent 10a346a
commit 6e1d40b
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp
@@ -228,14 +228,16 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
     if (s < s_bound_start) {
       continue;
     }
+
     if (path_bound.empty()) {
       const auto interpolated_point =
         lanelet::geometry::interpolatedPointAtDistance(lanelet_bound, s_bound_start);
       path_bound.push_back(
         lanelet::utils::conversion::toGeomMsgPt(lanelet::utils::to3D(interpolated_point)));
-      continue;
+    } else {
+      path_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(*it));
     }
-    path_bound.push_back(lanelet::utils::conversion::toGeomMsgPt(*it));
+
     if (s >= s_bound_end) {
       const auto interpolated_point =
         lanelet::geometry::interpolatedPointAtDistance(lanelet_bound, s_bound_end);