fix(goal_planner): fix goal planner execution (autowarefoundation#3893)

* fix(goal_planner): fix goal planner execution

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -276,37 +276,59 @@ bool GoalPlannerModule::isExecutionRequested() const
   if (current_state_ == ModuleStatus::RUNNING) {
     return true;
   }
-  const auto & route_handler = planner_data_->route_handler;
 
-  // if current position is far from goal, do not execute pull over
+  const auto & route_handler = planner_data_->route_handler;
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   const Pose & goal_pose = route_handler->getGoalPose();
+
+  // if goal is shoulder lane, allow goal modification
+  allow_goal_modification_ =
+    route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder();
+
+  // check if goal_pose is in current_lanes.
   lanelet::ConstLanelet current_lane{};
   const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
   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 neghibor shoulder lane
+  const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
+    lanelet::ConstLanelet neighbor_shoulder_lane{};
+    for (const auto & lane : current_lanes) {
+      const bool has_shoulder_lane =
+        left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane, &neighbor_shoulder_lane)
+                           : route_handler->getRightShoulderLanelet(lane, &neighbor_shoulder_lane);
+      if (has_shoulder_lane && lanelet::utils::isInLanelet(goal_pose, neighbor_shoulder_lane)) {
+        return true;
+      }
+    }
+    return false;
+  });
+
+  // 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_lanes && !goal_is_in_current_shoulder_lanes) {
+    return false;
+  }
+
+  // if goal arc coordinates can be calculated, check if goal is in request_length
   const double self_to_goal_arc_length =
     utils::getSignedDistance(current_pose, goal_pose, current_lanes);
-  allow_goal_modification_ =
-    route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder();
   const double request_length =
     allow_goal_modification_ ? calcModuleRequestLength() : parameters_->minimum_request_length;
-  const double backward_goal_search_length =
-    allow_goal_modification_ ? parameters_->backward_goal_search_length : 0.0;
-  if (
-    self_to_goal_arc_length < -backward_goal_search_length ||
-    self_to_goal_arc_length > request_length) {
+  if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) {
+    // if current position is far from goal or behind goal, do not execute goal_planner
     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 (!allow_goal_modification_) {
-    // check if goal_pose is in current_lanes.
-    return std::any_of(
-      current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) {
-        return lanelet::utils::isInLanelet(goal_pose, current_lane);
-      });
+    return goal_is_in_current_lanes;
   }
 
   // if (A) or (B) is met execute pull over