fix(goal_planner): check usage of bus_stop_area by goal_pose (autowarefoundation#10041)

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

@@ -338,6 +338,8 @@ class GoalPlannerModule : public SceneModuleInterface
   std::shared_ptr<GoalSearcherBase> goal_searcher_;
   GoalCandidates goal_candidates_{};
 
+  bool use_bus_stop_area_{false};
+
   // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on
   // onFreespaceParkingTimer thread storage may point to while calculation.
   // onTimer/onFreespaceParkingTimer and their callees MUST NOT use this
@@ -362,7 +364,7 @@ class GoalPlannerModule : public SceneModuleInterface
   mutable GoalPlannerDebugData debug_data_;
 
   // goal seach
-  GoalCandidates generateGoalCandidates() const;
+  GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) const;
 
   /*
    * state transitions and plan function used in each state

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

@@ -30,7 +30,8 @@ class GoalSearcher : public GoalSearcherBase
 public:
   GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
 
-  GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
+  GoalCandidates search(
+    const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area) override;
   void update(
     GoalCandidates & goal_candidates,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,

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

@@ -47,7 +47,8 @@ class GoalSearcherBase
   virtual ~GoalSearcherBase() = default;
 
   MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
-  virtual GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) = 0;
+  virtual GoalCandidates search(
+    const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area) = 0;
   virtual void update(
     [[maybe_unused]] GoalCandidates & goal_candidates,
     [[maybe_unused]] const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,

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

@@ -33,9 +33,11 @@ class LaneParkingRequest
 public:
   LaneParkingRequest(
     const autoware::universe_utils::LinearRing2d & vehicle_footprint,
-    const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output)
+    const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output,
+    const bool use_bus_stop_area)
   : vehicle_footprint_(vehicle_footprint),
     goal_candidates_(goal_candidates),
+    use_bus_stop_area_(use_bus_stop_area),
     upstream_module_output_(upstream_module_output)
   {
   }
@@ -48,6 +50,7 @@ class LaneParkingRequest
 
   const autoware::universe_utils::LinearRing2d vehicle_footprint_;
   const GoalCandidates goal_candidates_;
+  const bool use_bus_stop_area_;
 
   const std::shared_ptr<PlannerData> & get_planner_data() const { return planner_data_; }
   const ModuleStatus & get_current_status() const { return current_status_; }

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -319,6 +319,7 @@ void LaneParkingPlanner::onTimer()
   const auto & pull_over_path_opt = local_request.get_pull_over_path();
   const auto & prev_data = local_request.get_prev_data();
   const auto trigger_thread_on_approach = local_request.trigger_thread_on_approach();
+  const auto use_bus_stop_area = local_request.use_bus_stop_area_;
 
   if (!trigger_thread_on_approach) {
     return;
@@ -385,7 +386,7 @@ void LaneParkingPlanner::onTimer()
   std::vector<PullOverPath> path_candidates{};
   std::optional<Pose> closest_start_pose{};
   std::optional<std::vector<size_t>> sorted_indices_opt{std::nullopt};
-  if (parameters_.bus_stop_area.use_bus_stop_area && switch_bezier_) {
+  if (use_bus_stop_area && switch_bezier_) {
     bezier_planning_helper(
       local_planner_data, goal_candidates, upstream_module_output, current_lanes,
       closest_start_pose, path_candidates, sorted_indices_opt);
@@ -658,7 +659,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
     std::lock_guard<std::mutex> guard(lane_parking_mutex_);
     if (!lane_parking_request_) {
       lane_parking_request_.emplace(
-        vehicle_footprint_, goal_candidates_, getPreviousModuleOutput());
+        vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(), use_bus_stop_area_);
     }
     // 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
@@ -713,7 +714,19 @@ void GoalPlannerModule::updateData()
 
   // update goal searcher and generate goal candidates
   if (goal_candidates_.empty()) {
-    goal_candidates_ = generateGoalCandidates();
+    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);
+    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 &&
+      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;
+          return boost::geometry::within(
+            universe_utils::Point2d{goal_position.x, goal_position.y}, area);
+        });
+    goal_candidates_ = generateGoalCandidates(use_bus_stop_area_);
   }
 
   const lanelet::ConstLanelets current_lanes =
@@ -982,14 +995,14 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte
   return true;
 }
 
-GoalCandidates GoalPlannerModule::generateGoalCandidates() const
+GoalCandidates GoalPlannerModule::generateGoalCandidates(const bool use_bus_stop_area) const
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
   if (utils::isAllowedGoalModification(route_handler)) {
-    return goal_searcher_->search(planner_data_);
+    return goal_searcher_->search(planner_data_, use_bus_stop_area);
   }
 
   // NOTE:

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -100,7 +100,8 @@ GoalSearcher::GoalSearcher(
 {
 }
 
-GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & planner_data)
+GoalCandidates GoalSearcher::search(
+  const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area)
 {
   GoalCandidates goal_candidates{};
 
@@ -118,7 +119,7 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
   const double forward_length = parameters_.forward_goal_search_length;
   const double backward_length = parameters_.backward_goal_search_length;
   const double margin_from_boundary = parameters_.margin_from_boundary;
-  const bool use_bus_stop_area = parameters_.bus_stop_area.use_bus_stop_area;
+
   const double lateral_offset_interval = use_bus_stop_area
                                            ? parameters_.bus_stop_area.lateral_offset_interval
                                            : parameters_.lateral_offset_interval;