fix(behavior_path_planner): fix sampling warning (autowarefoundation#…

…4216)

* fix(behavior_path_planner): fix sampling warning

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

planning/behavior_path_planner/src/utils/path_utils.cpp

@@ -108,13 +108,17 @@ PathWithLaneId resamplePathWithSpline(
 
   std::vector<double> s_out = s_in;
 
+  // sampling from interval distance
   const auto start_s = std::max(target_section.first, 0.0);
   const auto end_s = std::min(target_section.second, s_vec.back());
   for (double s = start_s; s < end_s; s += interval) {
     if (!find_almost_same_values(s_out, s)) {
       s_out.push_back(s);
     }
   }
+  if (!find_almost_same_values(s_out, end_s)) {
+    s_out.push_back(end_s);
+  }
 
   // Insert Stop Point
   const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path);
@@ -133,7 +137,8 @@ PathWithLaneId resamplePathWithSpline(
     }
   }
 
-  if (s_out.empty()) {
+  // spline resample required more than 2 points for yaw angle calculation
+  if (s_out.size() < 2) {
     return path;
   }