chore: cherry-pick/bus_stop_area

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

@@ -288,17 +288,7 @@ class GoalPlannerModule : public SceneModuleInterface
       getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished");
   }
 
-  void updateModuleParams(const std::any & parameters) override
-  {
-    parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
-    if (parameters_->safety_check_params.enable_safety_check) {
-      ego_predicted_path_params_ =
-        std::make_shared<EgoPredictedPathParams>(parameters_->ego_predicted_path_params);
-      objects_filtering_params_ =
-        std::make_shared<ObjectsFilteringParams>(parameters_->objects_filtering_params);
-      safety_check_params_ = std::make_shared<SafetyCheckParams>(parameters_->safety_check_params);
-    }
-  }
+  void updateModuleParams([[maybe_unused]] const std::any & parameters) override {}
 
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
@@ -308,69 +298,48 @@ class GoalPlannerModule : public SceneModuleInterface
   void updateData() override;
 
   void postProcess() override;
-  void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
   {
   }
   CandidateOutput planCandidate() const override { return CandidateOutput{}; }
 
 private:
-  std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();
+  const GoalPlannerParameters parameters_;
+  const EgoPredictedPathParams & ego_predicted_path_params_ = parameters_.ego_predicted_path_params;
+  const ObjectsFilteringParams & objects_filtering_params_ = parameters_.objects_filtering_params;
+  const SafetyCheckParams safety_check_params_ = parameters_.safety_check_params;
+
+  const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+  const autoware::universe_utils::LinearRing2d vehicle_footprint_;
+
+  const bool left_side_parking_;
 
   bool trigger_thread_on_approach_{false};
+  // pre-generate lane parking paths in a separate thread
+  rclcpp::TimerBase::SharedPtr lane_parking_timer_;
+  rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
+  std::atomic<bool> is_lane_parking_cb_running_;
   // NOTE: never access to following variables except in updateData()!!!
   std::mutex lane_parking_mutex_;
   std::optional<LaneParkingRequest> lane_parking_request_;
   LaneParkingResponse lane_parking_response_;
-
+  // generate freespace parking paths in a separate thread
+  rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
+  rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
+  std::atomic<bool> is_freespace_parking_cb_running_;
   std::mutex freespace_parking_mutex_;
   std::optional<FreespaceParkingRequest> freespace_parking_request_;
   FreespaceParkingResponse freespace_parking_response_;
 
-  /*
-   * state transitions and plan function used in each state
-   *
-   * +--------------------------+
-   * | RUNNING                  |
-   * | planPullOverAsCandidate()|
-   * +------------+-------------+
-   *              | hasDecidedPath()
-   *  2           v
-   * +--------------------------+
-   * | WAITING_APPROVAL         |
-   * | planPullOverAsCandidate()|
-   * +------------+-------------+
-   *              | isActivated()
-   *  3           v
-   * +--------------------------+
-   * | RUNNING                  |
-   * | planPullOverAsOutput()   |
-   * +--------------------------+
-   */
-
-  // The start_planner activates when it receives a new route,
-  // so there is no need to terminate the goal planner.
-  // If terminating it, it may switch to lane following and could generate an inappropriate path.
-  bool canTransitSuccessState() override { return false; }
-  bool canTransitFailureState() override { return false; }
-
-  mutable StartGoalPlannerData goal_planner_data_;
-
-  std::shared_ptr<GoalPlannerParameters> parameters_;
-
-  mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
-  mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
-  mutable std::shared_ptr<SafetyCheckParams> safety_check_params_;
-
-  autoware::vehicle_info_utils::VehicleInfo vehicle_info_{};
-
   std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
 
   // goal searcher
   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
@@ -380,8 +349,6 @@ class GoalPlannerModule : public SceneModuleInterface
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stopped_;
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stuck_;
 
-  autoware::universe_utils::LinearRing2d vehicle_footprint_;
-
   std::optional<PullOverContextData> context_data_{std::nullopt};
   // path_decision_controller is updated in updateData(), and used in plan()
   PathDecisionStateController path_decision_controller_{getLogger()};
@@ -393,24 +360,40 @@ class GoalPlannerModule : public SceneModuleInterface
   // ego may exceed the stop distance, so add a buffer
   const double stop_distance_buffer_{2.0};
 
-  // for parking policy
-  bool left_side_parking_{true};
-
-  // pre-generate lane parking paths in a separate thread
-  rclcpp::TimerBase::SharedPtr lane_parking_timer_;
-  rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
-  std::atomic<bool> is_lane_parking_cb_running_;
-
-  // generate freespace parking paths in a separate thread
-  rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
-  rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
-  std::atomic<bool> is_freespace_parking_cb_running_;
-
   // debug
   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
+   *
+   * +--------------------------+
+   * | RUNNING                  |
+   * | planPullOverAsCandidate()|
+   * +------------+-------------+
+   *              | hasDecidedPath()
+   *  2           v
+   * +--------------------------+
+   * | WAITING_APPROVAL         |
+   * | planPullOverAsCandidate()|
+   * +------------+-------------+
+   *              | isActivated()
+   *  3           v
+   * +--------------------------+
+   * | RUNNING                  |
+   * | planPullOverAsOutput()   |
+   * +--------------------------+
+   */
+
+  // The start_planner activates when it receives a new route,
+  // so there is no need to terminate the goal planner.
+  // If terminating it, it may switch to lane following and could generate an inappropriate path.
+  bool canTransitSuccessState() override { return false; }
+  bool canTransitFailureState() override { return false; }
+
+  std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();
 
   // stop or decelerate
   void deceleratePath(PullOverPath & pull_over_path) const;
@@ -427,7 +410,7 @@ class GoalPlannerModule : public SceneModuleInterface
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
 
   // status
-  bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
+  bool hasFinishedCurrentPath(const PullOverContextData & ctx_data) const;
   double calcModuleRequestLength() const;
   void decideVelocity(PullOverPath & pull_over_path);
   void updateStatus(const BehaviorModuleOutput & output);
@@ -480,9 +463,6 @@ class GoalPlannerModule : public SceneModuleInterface
   std::pair<double, double> calcDistanceToPathChange(
     const PullOverContextData & context_data) const;
 
-  // safety check
-  void initializeSafetyCheckParameters();
-  SafetyCheckParams createSafetyCheckParams() const;
   /*
   void updateSafetyCheckTargetObjectsData(
     const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
@@ -494,11 +474,8 @@ class GoalPlannerModule : public SceneModuleInterface
    */
   std::pair<bool, utils::path_safety_checker::CollisionCheckDebugMap> isSafePath(
     const std::shared_ptr<const PlannerData> planner_data, const bool found_pull_over_path,
-    const std::optional<PullOverPath> & pull_over_path_opt, const PathDecisionState & prev_data,
-    const GoalPlannerParameters & parameters,
-    const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
-    const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
-    const std::shared_ptr<SafetyCheckParams> & safety_check_params) const;
+    const std::optional<PullOverPath> & pull_over_path_opt,
+    const PathDecisionState & prev_data) const;
 
   // debug
   void setDebugData(const PullOverContextData & context_data);

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_; }