Revert "feat(goal_planner): execute goal planner if previous module p…

…ath terminal is pull over neighboring lane (autowarefoundation#8715)"

This reverts commit 11cb352.

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md

@@ -107,7 +107,7 @@ If the target path contains a goal, modify the points of the path so that the pa
 - Route is set with `allow_goal_modification=true` .
   - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
   - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
-- The terminal point of the current path is in the same lane sequence as the goal. If goal is on the road shoulder, then it is in the adjacent road lane sequence.
+- ego-vehicle is in the same lane as the goal.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929950-989ca6c3-d48c-4bb5-81e5-e8d6a38911aa.png" width="600">
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -581,25 +581,15 @@ bool GoalPlannerModule::isExecutionRequested() const
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   const Pose goal_pose = route_handler->getOriginalGoalPose();
 
-  // check if goal_pose is in current_lanes or neighboring road lanes
+  // check if goal_pose is in current_lanes.
+  lanelet::ConstLanelet current_lane{};
+  // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
   const lanelet::ConstLanelets current_lanes =
     utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
-  const auto getNeighboringLane =
-    [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
-    return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true)
-                              : route_handler->getRightLanelet(lane, false, true);
-  };
-  lanelet::ConstLanelets goal_check_lanes = current_lanes;
-  for (const auto & lane : current_lanes) {
-    auto neighboring_lane = getNeighboringLane(lane);
-    while (neighboring_lane) {
-      goal_check_lanes.push_back(neighboring_lane.value());
-      neighboring_lane = getNeighboringLane(neighboring_lane.value());
-    }
-  }
-  const bool goal_is_in_current_segment_lanes = std::any_of(
-    goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) {
-      return lanelet::utils::isInLanelet(goal_pose, lane);
+  lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &current_lane);
+  const bool goal_is_in_current_lanes = std::any_of(
+    current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) {
+      return lanelet::utils::isInLanelet(goal_pose, current_lane);
     });
 
   // check that goal is in current neighbor shoulder lane
@@ -616,15 +606,15 @@ bool GoalPlannerModule::isExecutionRequested() const
 
   // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner,
   // because goal arc coordinates cannot be calculated.
-  if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) {
+  if (!goal_is_in_current_lanes && !goal_is_in_current_shoulder_lanes) {
     return false;
   }
 
   // if goal modification is not allowed
   // 1) goal_pose is in current_lanes, plan path to the original fixed goal
   // 2) goal_pose is NOT in current_lanes, do not execute goal_planner
   if (!utils::isAllowedGoalModification(route_handler)) {
-    return goal_is_in_current_segment_lanes;
+    return goal_is_in_current_lanes;
   }
 
   // if goal arc coordinates can be calculated, check if goal is in request_length
@@ -646,12 +636,7 @@ bool GoalPlannerModule::isExecutionRequested() const
     parameters_->forward_goal_search_length);
   lanelet::ConstLanelet target_lane{};
   lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane);
-
-  lanelet::ConstLanelet previous_module_terminal_lane{};
-  route_handler->getClosestLaneletWithinRoute(
-    getPreviousModuleOutput().path.points.back().point.pose, &previous_module_terminal_lane);
-
-  if (!isCrossingPossible(previous_module_terminal_lane, target_lane)) {
+  if (!isCrossingPossible(current_lane, target_lane)) {
     return false;
   }
 
@@ -2272,10 +2257,14 @@ bool GoalPlannerModule::isCrossingPossible(
     end_lane_sequence = route_handler->getLaneletSequence(end_lane, dist, dist, false);
   }
 
-  const auto getNeighboringLane =
+  // Lambda function to get the neighboring lanelet based on left_side_parking_
+  auto getNeighboringLane =
     [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
-    return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true)
-                              : route_handler->getRightLanelet(lane, false, true);
+    const auto neighboring_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
+                                                     : route_handler->getRightShoulderLanelet(lane);
+    if (neighboring_lane) return neighboring_lane;
+    return left_side_parking_ ? route_handler->getLeftLanelet(lane)
+                              : route_handler->getRightLanelet(lane);
   };
 
   // Iterate through start_lane_sequence to find a path to end_lane_sequence