From c6223578c8372cb1417638b902684d3f4545c2a6 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 11 Jan 2023 13:21:51 +0900 Subject: [PATCH] feat(lane_change): update resamplePath function for target section (#2622) (#239) * feat(lane_change): update resamplePath function for target section Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * fix: apply pre-commit Signed-off-by: tomoya.kimura Signed-off-by: Takamasa Horibe Signed-off-by: tomoya.kimura Co-authored-by: tomoya.kimura Signed-off-by: Takamasa Horibe Signed-off-by: tomoya.kimura Co-authored-by: Takamasa Horibe --- .../include/behavior_path_planner/path_utilities.hpp | 12 +++++++++++- .../behavior_path_planner/src/path_utilities.cpp | 12 +++++++----- .../src/scene_module/lane_change/util.cpp | 3 ++- 3 files changed, 20 insertions(+), 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 index 2d6f44633cc5d..09269e8cb7456 100644 --- 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 calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, const size_t end = std::numeric_limits::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 target_section = {0.0, std::numeric_limits::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 index 457687a6667d3..659006752f45e 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -56,7 +56,8 @@ std::vector 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 target_section) { if (path.points.size() < 2) { return path; @@ -96,16 +97,17 @@ PathWithLaneId resamplePathWithSpline( std::vector 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 index ffad3231eee05..8c893c9dcf63f 100644 --- 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(