refactor(goal planner): hold modified_goal in PullOverPath ,copy modified goal once from background thread

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp

@@ -37,8 +37,9 @@ class FreespacePullOver : public PullOverPlannerBase
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
 
   std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
 protected:
   const double velocity_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp

@@ -43,8 +43,9 @@ class GeometricPullOver : public PullOverPlannerBase
   Pose getCl() const { return planner_.getCl(); }
 
   std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
   std::vector<PullOverPath> generatePullOverPaths(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp

@@ -15,6 +15,7 @@
 #pragma once
 
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
 #include "autoware/behavior_path_planner_common/data_manager.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -41,19 +42,20 @@ struct PullOverPath
 {
 public:
   static std::optional<PullOverPath> create(
-    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
+    const PullOverPlannerType & type, const size_t id,
     const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose,
-    const Pose & end_pose,
+    const GoalCandidate & modified_goal_pose,
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPath(const PullOverPath & other);
   PullOverPath & operator=(const PullOverPath & other) = default;
 
   PullOverPlannerType type() const { return type_; }
-  size_t goal_id() const { return goal_id_; }
+  size_t goal_id() const { return modified_goal_pose_.id; }
   size_t id() const { return id_; }
   Pose start_pose() const { return start_pose_; }
-  Pose end_pose() const { return end_pose_; }
+  Pose end_pose() const { return modified_goal_pose_.goal_pose; }
+  GoalCandidate modified_goal_pose() const { return modified_goal_pose_; }
 
   std::vector<PathWithLaneId> & partial_paths() { return partial_paths_; }
   const std::vector<PathWithLaneId> & partial_paths() const { return partial_paths_; }
@@ -86,19 +88,18 @@ struct PullOverPath
 
 private:
   PullOverPath(
-    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
-    const Pose & start_pose, const Pose & end_pose,
-    const std::vector<PathWithLaneId> & partial_paths, const PathWithLaneId & full_path,
-    const PathWithLaneId & parking_path, const std::vector<double> & full_path_curvatures,
+    const PullOverPlannerType & type, const size_t id, const Pose & start_pose,
+    const GoalCandidate & modified_goal_pose, const std::vector<PathWithLaneId> & partial_paths,
+    const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
+    const std::vector<double> & full_path_curvatures,
     const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
     const double parking_path_max_curvature,
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPlannerType type_;
-  size_t goal_id_;
+  GoalCandidate modified_goal_pose_;
   size_t id_;
   Pose start_pose_;
-  Pose end_pose_;
 
   std::vector<PathWithLaneId> partial_paths_;
   PathWithLaneId full_path_;
@@ -126,8 +127,9 @@ class PullOverPlannerBase
 
   virtual PullOverPlannerType getPlannerType() const = 0;
   virtual std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) = 0;
 
 protected:
   const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp

@@ -36,8 +36,9 @@ class ShiftPullOver : public PullOverPlannerBase
     const LaneDepartureChecker & lane_departure_checker);
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; };
   std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
 protected:
   PathWithLaneId generateReferencePath(
@@ -46,10 +47,10 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PathWithLaneId> cropPrevModulePath(
     const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
+    const GoalCandidate & goal_candidate, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose,
-    const double lateral_jerk) const;
+    const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);
   static std::vector<double> splineTwoPoints(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -303,8 +303,7 @@
                                     const std::shared_ptr<PullOverPlannerBase> & planner,
                                     const GoalCandidate & goal_candidate) {
     const auto pull_over_path = planner->plan(
-      goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output,
-      goal_candidate.goal_pose);
+      goal_candidate, path_candidates.size(), local_planner_data, previous_module_output);
     if (pull_over_path) {
       // calculate absolute maximum curvature of parking path(start pose to end pose) for path
       // priority
@@ -803,9 +802,8 @@
       continue;
     }
     const auto freespace_path = freespace_planner_->plan(
-      goal_candidate.id, 0, planner_data,
-      BehaviorModuleOutput{},  // NOTE: not used so passing {} is OK
-      goal_candidate.goal_pose);
+      goal_candidate, 0, planner_data, BehaviorModuleOutput{}  // NOTE: not used so passing {} is OK
+    );
     if (!freespace_path) {
       continue;
     }
@@ -1824,7 +1822,7 @@
     return ignore_signal_.value() == id;
   };
 
-  const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+  const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) {
     return is_ignore ? std::make_optional(id) : std::nullopt;
   };
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp

@@ -57,96 +57,98 @@
 }
 
 std::optional<PullOverPath> FreespacePullOver::plan(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose)
+  const GoalCandidate & modified_goal_pose, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
+  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output)
 {
   const Pose & current_pose = planner_data->self_odometry->pose.pose;
 
   planner_->setMap(*planner_data->costmap);
 
   // offset goal pose to make straight path near goal for improving parking precision
   // todo: support straight path when using back
   constexpr double straight_distance = 1.0;
+  const auto & goal_pose = modified_goal_pose.goal_pose;
   const Pose end_pose =
     use_back_ ? goal_pose
               : autoware::universe_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0);
 
   try {
     if (!planner_->makePlan(current_pose, end_pose)) {
       return {};
     }
   } catch (const std::exception & e) {
     return {};
   }
 
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
     /*forward_only_in_route*/ false);
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *(planner_data->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
     parameters_.forward_goal_search_length);
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }
   const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes);
 
   PathWithLaneId path =
     utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes);
   const auto reverse_indices = utils::getReversingIndices(path);
   std::vector<PathWithLaneId> partial_paths = utils::dividePath(path, reverse_indices);
 
   // remove points which are near the goal
   PathWithLaneId & last_path = partial_paths.back();
   const double th_goal_distance = 1.0;
   for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) {
     size_t index = std::distance(last_path.points.begin(), it);
     if (index == 0) continue;
     const double distance =
       autoware::universe_utils::calcDistance2d(end_pose.position, it->point.pose.position);
     if (distance < th_goal_distance) {
       last_path.points.erase(it, last_path.points.end());
       break;
     }
   }
 
   // add PathPointWithLaneId to last path
   auto addPose = [&last_path](const Pose & pose) {
     PathPointWithLaneId p = last_path.points.back();
     p.point.pose = pose;
     last_path.points.push_back(p);
   };
 
   if (use_back_) {
     addPose(end_pose);
   } else {
     // add interpolated poses
     auto addInterpolatedPoses = [&addPose](const Pose & pose1, const Pose & pose2) {
       constexpr double interval = 0.5;
       std::vector<Pose> interpolated_poses = utils::interpolatePose(pose1, pose2, interval);
       for (const auto & pose : interpolated_poses) {
         addPose(pose);
       }
     };
     addInterpolatedPoses(last_path.points.back().point.pose, end_pose);
     addPose(end_pose);
     addInterpolatedPoses(end_pose, goal_pose);
     addPose(goal_pose);
   }
 
   std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel{};
   pairs_terminal_velocity_and_accel.resize(partial_paths.size());
   utils::parking_departure::modifyVelocityByDirection(
     partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0);
 
   // Check if driving forward for each path, return empty if not
   for (auto & partial_path : partial_paths) {
     if (!autoware::motion_utils::isDrivingForward(partial_path.points)) {
       return {};
     }
   }
 
   auto pull_over_path_opt = PullOverPath::create(
-    getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose,
+    getPlannerType(), id, partial_paths, current_pose, modified_goal_pose,
     pairs_terminal_velocity_and_accel);
   if (!pull_over_path_opt) {
     return {};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -38,11 +38,13 @@ GeometricPullOver::GeometricPullOver(
 }
 
 std::optional<PullOverPath> GeometricPullOver::plan(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose)
+  const GoalCandidate & modified_goal_pose, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
+  [[maybe_unused]] const BehaviorModuleOutput & previous_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
 
+  const auto & goal_pose = modified_goal_pose.goal_pose;
   // prepare road nad shoulder lanes
   const auto road_lanes = utils::getExtendedCurrentLanes(
     planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
@@ -73,8 +75,8 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {};
 
   auto pull_over_path_opt = PullOverPath::create(
-    getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(),
-    planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel());
+    getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose,
+    planner_.getPairsTerminalVelocityAndAccel());
   if (!pull_over_path_opt) {
     return {};
   }

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp

@@ -18,75 +18,75 @@
 {
 
 std::optional<PullOverPath> PullOverPath::create(
-  const PullOverPlannerType & type, const size_t goal_id, const size_t id,
-  const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose, const Pose & end_pose,
+  const PullOverPlannerType & type, const size_t id,
+  const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose,
+  const GoalCandidate & modified_goal_pose,
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel)
 {
   if (partial_paths.empty()) {
     return std::nullopt;
   }
   PathWithLaneId path{};
   for (size_t i = 0; i < partial_paths.size(); ++i) {
     if (i == 0) {
       path.points.insert(
         path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end());
     } else {
       // skip overlapping point
       path.points.insert(
         path.points.end(), next(partial_paths.at(i).points.begin()),
         partial_paths.at(i).points.end());
     }
   }
   PathWithLaneId full_path{};
   full_path.points = autoware::motion_utils::removeOverlapPoints(path.points);
   if (full_path.points.size() < 3) {
     return std::nullopt;
   }
 
   const size_t start_idx =
     autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position);
 
   PathWithLaneId parking_path{};
   std::copy(
     full_path.points.begin() + start_idx, full_path.points.end(),
     std::back_inserter(parking_path.points));
 
   if (parking_path.points.size() < 3) {
     return std::nullopt;
   }
 
   const auto calculateCurvaturesAndMax =
     [](const auto & path) -> std::pair<std::vector<double>, double> {
     std::vector<double> curvatures = autoware::motion_utils::calcCurvature(path.points);
     double max_curvature = 0.0;
     if (!curvatures.empty()) {
       max_curvature = std::abs(*std::max_element(
         curvatures.begin(), curvatures.end(),
         [](const double & a, const double & b) { return std::abs(a) < std::abs(b); }));
     }
     return std::make_pair(curvatures, max_curvature);
   };
 
   std::vector<double> full_path_curvatures{};
   std::vector<double> parking_path_curvatures{};
   double full_path_max_curvature{};
   double parking_path_max_curvature{};
   std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path);
   std::tie(parking_path_curvatures, parking_path_max_curvature) =
     calculateCurvaturesAndMax(parking_path);
 
   return PullOverPath(
-    type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path,
+    type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path,
     full_path_curvatures, parking_path_curvatures, full_path_max_curvature,
     parking_path_max_curvature, pairs_terminal_velocity_and_accel);
 }
 
 PullOverPath::PullOverPath(const PullOverPath & other)
 : type_(other.type_),
-  goal_id_(other.goal_id_),
+  modified_goal_pose_(other.modified_goal_pose_),
   id_(other.id_),
   start_pose_(other.start_pose_),
-  end_pose_(other.end_pose_),
   partial_paths_(other.partial_paths_),
   full_path_(other.full_path_),
   parking_path_(other.parking_path_),
@@ -100,18 +100,17 @@
 }
 
 PullOverPath::PullOverPath(
-  const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose,
-  const Pose & end_pose, const std::vector<PathWithLaneId> & partial_paths,
+  const PullOverPlannerType & type, const size_t id, const Pose & start_pose,
+  const GoalCandidate & modified_goal_pose, const std::vector<PathWithLaneId> & partial_paths,
   const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
   const std::vector<double> & full_path_curvatures,
   const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
   const double parking_path_max_curvature,
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel)
 : type_(type),
-  goal_id_(goal_id),
+  modified_goal_pose_(modified_goal_pose),
   id_(id),
   start_pose_(start_pose),
-  end_pose_(end_pose),
   partial_paths_(partial_paths),
   full_path_(full_path),
   parking_path_(parking_path),

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -35,31 +35,32 @@
 {
 }
 std::optional<PullOverPath> ShiftPullOver::plan(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-  const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose)
+  const GoalCandidate & modified_goal_pose, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
+  const BehaviorModuleOutput & previous_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
   const double min_jerk = parameters_.minimum_lateral_jerk;
   const double max_jerk = parameters_.maximum_lateral_jerk;
   const double backward_search_length = parameters_.backward_goal_search_length;
   const double forward_search_length = parameters_.forward_goal_search_length;
   const int shift_sampling_num = parameters_.shift_sampling_num;
   const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
   const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
     previous_module_output.path, planner_data, backward_search_length, forward_search_length,
     /*forward_only_in_route*/ false);
 
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, backward_search_length, forward_search_length);
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }
 
   // find safe one from paths with different jerk
   for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) {
     const auto pull_over_path = generatePullOverPath(
-      goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose,
+      modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes,
       lateral_jerk);
     if (!pull_over_path) continue;
     return *pull_over_path;
@@ -127,137 +128,139 @@
 }
 
 std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
-  const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
+  const GoalCandidate & goal_candidate, const size_t id,
+  const std::shared_ptr<const PlannerData> planner_data,
   const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose,
-  const double lateral_jerk) const
+  const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const
 {
   const double pull_over_velocity = parameters_.pull_over_velocity;
   const double after_shift_straight_distance = parameters_.after_shift_straight_distance;
 
+  const auto & goal_pose = goal_candidate.goal_pose;
+
   // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy
   const Pose shift_end_pose =
     autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0);
 
   // calculate lateral shift of previous module path terminal pose from road lane reference path
   const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline(
     generateReferencePath(planner_data, road_lanes, shift_end_pose),
     parameters_.center_line_path_interval);
   const auto prev_module_path = utils::resamplePathWithSpline(
     previous_module_output.path, parameters_.center_line_path_interval);
   const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose;
 
   // process previous module path for path shifter input path
   // case1) extend path if shift end pose is behind of previous module path terminal pose
   // case2) crop path if shift end pose is ahead of previous module path terminal pose
   const auto processed_prev_module_path = std::invoke([&]() -> std::optional<PathWithLaneId> {
     const bool extend_previous_module_path =
       lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length >
       lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length;
     if (extend_previous_module_path) {  // case1
       // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it
       // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is
       // the role of the goal planner, and the intermediate zero velocity after extension is
       // unnecessary.
       return goal_planner_utils::extendPath(
         prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true);
     } else {  // case2
       return goal_planner_utils::cropPath(prev_module_path, shift_end_pose);
     }
   });
   if (!processed_prev_module_path || processed_prev_module_path->points.empty()) {
     return {};
   }
 
   // calculate shift length
   const Pose & shift_end_pose_prev_module_path =
     processed_prev_module_path->points.back().point.pose;
   const double shift_end_road_to_target_distance =
     autoware::universe_utils::inverseTransformPoint(
       shift_end_pose.position, shift_end_pose_prev_module_path)
       .y;
 
   // calculate shift start pose on road lane
   const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk(
     shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity);
   const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength(
     processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance);
   const auto shift_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
     processed_prev_module_path->points, shift_end_pose_prev_module_path.position,
     -before_shifted_pull_over_distance);
 
   // set path shifter and generate shifted path
   PathShifter path_shifter{};
   path_shifter.setPath(processed_prev_module_path.value());
   ShiftLine shift_line{};
   shift_line.start = *shift_start_pose;
   shift_line.end = shift_end_pose;
   shift_line.end_shift_length = shift_end_road_to_target_distance;
   path_shifter.addShiftLine(shift_line);
   ShiftedPath shifted_path{};
   const bool offset_back = true;  // offset front side from reference path
   if (!path_shifter.generate(&shifted_path, offset_back)) {
     return {};
   }
   shifted_path.path.points = autoware::motion_utils::removeOverlapPoints(shifted_path.path.points);
   autoware::motion_utils::insertOrientation(shifted_path.path.points, true);
 
   // set same orientation, because the reference center line orientation is not same to the
   shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation;
 
   // for debug. result of shift is not equal to the target
   const Pose actual_shift_end_pose = shifted_path.path.points.back().point.pose;
 
   // interpolate between shift end pose to goal pose
   std::vector<Pose> interpolated_poses =
     utils::interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5);
   for (const auto & pose : interpolated_poses) {
     PathPointWithLaneId p = shifted_path.path.points.back();
     p.point.pose = pose;
     shifted_path.path.points.push_back(p);
   }
 
   // set goal pose with velocity 0
   {
     PathPointWithLaneId p{};
     p.point.longitudinal_velocity_mps = 0.0;
     p.point.pose = goal_pose;
     p.lane_ids = shifted_path.path.points.back().lane_ids;
     for (const auto & lane : shoulder_lanes) {
       p.lane_ids.push_back(lane.id());
     }
     shifted_path.path.points.push_back(p);
   }
 
   // set lane_id and velocity to shifted_path
   for (size_t i = path_shifter.getShiftLines().front().start_idx;
        i < shifted_path.path.points.size() - 1; ++i) {
     auto & point = shifted_path.path.points.at(i);
     // set velocity
     point.point.longitudinal_velocity_mps =
       std::min(point.point.longitudinal_velocity_mps, static_cast<float>(pull_over_velocity));
 
     // add target lanes to points after shift start
     // add road lane_ids if not found
     for (const auto id : shifted_path.path.points.back().lane_ids) {
       if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) {
         point.lane_ids.push_back(id);
       }
     }
     // add shoulder lane_id if not found
     for (const auto & lane : shoulder_lanes) {
       if (
         std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) ==
         point.lane_ids.end()) {
         point.lane_ids.push_back(lane.id());
       }
     }
   }
 
   // set pull over path
   auto pull_over_path_opt = PullOverPath::create(
-    getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start,
-    path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)});
+    getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start,
+    goal_candidate, {std::make_pair(pull_over_velocity, 0)});
 
   if (!pull_over_path_opt) {
     return {};