From c24ad46e156b0ff3564824e23936541f4834770e Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 10 May 2024 21:40:28 +0900 Subject: [PATCH] perf(side_shift): fix unupdated prev path that caused heavy interpolation (#6967) Signed-off-by: Maxime CLEMENT --- .../behavior_path_side_shift_module/scene.hpp | 1 - .../behavior_path_side_shift_module/src/scene.cpp | 14 ++++++-------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 1748f57a61bec..08c2345df5814 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -92,7 +92,6 @@ class SideShiftModule : public SceneModuleInterface // member PathWithLaneId refined_path_{}; PathWithLaneId reference_path_{}; - PathWithLaneId prev_reference_{}; lanelet::ConstLanelets current_lanelets_; std::shared_ptr parameters_; diff --git a/planning/behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp index 6df8c1ec629c9..36321050da1ad 100644 --- a/planning/behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_side_shift_module/src/scene.cpp @@ -202,9 +202,6 @@ void SideShiftModule::updateData() const auto reference_pose = prev_output_.shift_length.empty() ? planner_data_->self_odometry->pose.pose : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); - if (prev_reference_.points.empty()) { - prev_reference_ = getPreviousModuleOutput().path; - } if (getPreviousModuleOutput().reference_path.points.empty()) { return; } @@ -433,13 +430,14 @@ PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & orig const auto backward_length = std::max( planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); + const auto & prev_reference = getPreviousModuleOutput().path; const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPose().position); - const size_t prev_ego_idx = findNearestSegmentIndex( - prev_reference_.points, getPoint(original_path.points.at(orig_ego_idx))); + const size_t prev_ego_idx = + findNearestSegmentIndex(prev_reference.points, getPoint(original_path.points.at(orig_ego_idx))); size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) { - if (backward_length > calcSignedArcLength(prev_reference_.points, clip_idx, prev_ego_idx)) { + if (backward_length > calcSignedArcLength(prev_reference.points, clip_idx, prev_ego_idx)) { break; } clip_idx = i; @@ -448,8 +446,8 @@ PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & orig PathWithLaneId extended_path{}; { extended_path.points.insert( - extended_path.points.end(), prev_reference_.points.begin() + clip_idx, - prev_reference_.points.begin() + prev_ego_idx); + extended_path.points.end(), prev_reference.points.begin() + clip_idx, + prev_reference.points.begin() + prev_ego_idx); } {