fix(goal_planner): ignore use bus_stop_area flag if there are no BusStopArea on the pull over lanes (#10274)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -195,7 +195,7 @@ class LaneParkingPlanner
   bool switch_bezier_{false};
   void normal_pullover_planning_helper(
     const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
-    const BehaviorModuleOutput & upstream_module_output,
+    const BehaviorModuleOutput & upstream_module_output, const bool use_bus_stop_area,
     const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
     std::vector<PullOverPath> & path_candidates);
   void bezier_planning_helper(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -60,6 +60,8 @@ class GoalSearcher
 
   MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
 
+  bool bus_stop_area_available() const { return !bus_stop_area_polygons_.empty(); }
+
 private:
   GoalSearcher(
     const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -384,7 +384,7 @@ void LaneParkingPlanner::onTimer()
       closest_start_pose, path_candidates, sorted_indices_opt);
   } else {
     normal_pullover_planning_helper(
-      local_planner_data, goal_candidates, upstream_module_output, current_lanes,
+      local_planner_data, goal_candidates, upstream_module_output, use_bus_stop_area, current_lanes,
       closest_start_pose, path_candidates);
   }
 
@@ -405,7 +405,7 @@ void LaneParkingPlanner::onTimer()
 
 void LaneParkingPlanner::normal_pullover_planning_helper(
   const std::shared_ptr<PlannerData> planner_data, const GoalCandidates & goal_candidates,
-  const BehaviorModuleOutput & upstream_module_output,
+  const BehaviorModuleOutput & upstream_module_output, const bool use_bus_stop_area,
   const lanelet::ConstLanelets current_lanelets, std::optional<Pose> & closest_start_pose,
   std::vector<PullOverPath> & path_candidates)
 {
@@ -464,7 +464,7 @@ void LaneParkingPlanner::normal_pullover_planning_helper(
   if (closest_start_pose) {
     const auto original_pose = planner_data->route_handler->getOriginalGoalPose();
     if (
-      parameters_.bus_stop_area.use_bus_stop_area &&
+      use_bus_stop_area &&
       std::fabs(autoware_utils::normalize_radian(
         autoware_utils::get_rpy(original_pose).z -
         autoware_utils::get_rpy(closest_start_pose.value()).z)) > pull_over_angle_threshold) {
@@ -473,7 +473,7 @@ void LaneParkingPlanner::normal_pullover_planning_helper(
       path_candidates.clear();
       RCLCPP_INFO(getLogger(), "will generate Bezier Paths next");
     }
-  } else if (parameters_.bus_stop_area.use_bus_stop_area && path_candidates.size() == 0) {
+  } else if (use_bus_stop_area && path_candidates.size() == 0) {
     switch_bezier_ = true;
     RCLCPP_INFO(
       getLogger(), "Could not find any shift pull over paths, will generate Bezier Paths next");
@@ -649,8 +649,10 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
   {
     std::lock_guard<std::mutex> guard(lane_parking_mutex_);
     if (!lane_parking_request_) {
+      const auto & goal_searcher = goal_searcher_.value();
       lane_parking_request_.emplace(
-        vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(), use_bus_stop_area_);
+        vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(),
+        use_bus_stop_area_ && goal_searcher.bus_stop_area_available());
     }
     // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that
     // planner_data_ is not nullptr, so it is OK to copy as value
@@ -714,7 +716,7 @@ void GoalPlannerModule::updateData()
       parameters_.forward_goal_search_length);
     const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes);
     use_bus_stop_area_ =
-      parameters_.bus_stop_area.use_bus_stop_area &&
+      parameters_.bus_stop_area.use_bus_stop_area && goal_searcher.bus_stop_area_available() &&
       std::any_of(
         bus_stop_area_polygons.begin(), bus_stop_area_polygons.end(), [&](const auto & area) {
           const auto & goal_position = planner_data_->route_handler->getOriginalGoalPose().position;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -227,9 +227,8 @@ GoalCandidates GoalSearcher::search(
         vehicle_footprint_, autoware_utils::pose2transform(search_pose));
 
       if (
-        parameters_.bus_stop_area.use_bus_stop_area &&
-        !goal_planner_utils::isWithinAreas(
-          transformed_vehicle_footprint, bus_stop_area_polygons_)) {
+        use_bus_stop_area && !goal_planner_utils::isWithinAreas(
+                               transformed_vehicle_footprint, bus_stop_area_polygons_)) {
         continue;
       }