feat(lane_change): update resamplePath function for target section (a…

…utowarefoundation#2622) (#239) * feat(lane_change): update resamplePath function for target section Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * update Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix: apply pre-commit Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
tier4 · Jan 11, 2023 · c622357 · c622357
1 parent 9d736c4
commit c622357
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 7 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp
@@ -41,8 +41,18 @@ std::vector<double> calcPathArcLengthArray(
   const PathWithLaneId & path, const size_t start = 0,
   const size_t end = std::numeric_limits<size_t>::max(), const double offset = 0.0);
 
+/**
+ * @brief resample path by spline with constant interval distance
+ * @param [in] path original path to be resampled
+ * @param [in] interval constant interval distance
+ * @param [in] keep_input_points original points are kept in the resampled points
+ * @param [in] target_section target section defined by arclength if you want to resample a part of
+ * the path
+ * @return resampled path
+ */
 PathWithLaneId resamplePathWithSpline(
-  const PathWithLaneId & path, const double interval, const bool keep_input_points = false);
+  const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
+  const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});
 
 Path toPath(const PathWithLaneId & input);
 

diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp
@@ -56,7 +56,8 @@ std::vector<double> calcPathArcLengthArray(
  * @brief resamplePathWithSpline
  */
 PathWithLaneId resamplePathWithSpline(
-  const PathWithLaneId & path, const double interval, const bool keep_input_points)
+  const PathWithLaneId & path, const double interval, const bool keep_input_points,
+  const std::pair<double, double> target_section)
 {
   if (path.points.size() < 2) {
     return path;
@@ -96,16 +97,17 @@ PathWithLaneId resamplePathWithSpline(
 
   std::vector<double> s_out = s_in;
 
-  const double path_len = motion_utils::calcArcLength(transformed_path);
-  for (double s = 0.0; s < path_len; s += interval) {
+  const auto start_s = std::max(target_section.first, 0.0);
+  const auto end_s = std::min(target_section.second, motion_utils::calcArcLength(transformed_path));
+  for (double s = start_s; s < end_s; s += interval) {
     if (!has_almost_same_value(s_out, s)) {
       s_out.push_back(s);
     }
   }
 
   // Insert Terminal Point
-  if (!has_almost_same_value(s_out, path_len)) {
-    s_out.push_back(path_len);
+  if (!has_almost_same_value(s_out, end_s)) {
+    s_out.push_back(end_s);
   }
 
   // Insert Stop Point

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -511,7 +511,8 @@ PathWithLaneId getReferencePathFromTargetLane(
   constexpr auto resampling_dt{0.2};
   const auto resample_interval =
     std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt);
-  return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval);
+  return util::resamplePathWithSpline(
+    lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_distance});
 }
 
 PathWithLaneId getReferencePathFromTargetLane(