perf(side_shift): fix unupdated prev path that caused heavy interpola…

…tion (#6967) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
autowarefoundation · May 10, 2024 · c24ad46 · c24ad46
1 parent e218218
commit c24ad46
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 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
@@ -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<SideShiftParameters> parameters_;
 

diff --git 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);
   }
 
   {