refactor(goal_planner): refactor goal_searcher and goal_candidates

Signed-off-by: Mamoru Sobue <[email protected]>

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

@@ -15,7 +15,7 @@
 #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_
 
-#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp"
 #include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
 
@@ -56,7 +56,7 @@ class PathDecisionStateController
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const bool is_current_safe, const GoalPlannerParameters & parameters,
-    const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
+    const GoalSearcher & goal_searcher, const bool is_activated,
     const std::optional<PullOverPath> & pull_over_path,
     std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded);
 
@@ -77,7 +77,7 @@ class PathDecisionStateController
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const bool is_current_safe, const GoalPlannerParameters & parameters,
-    const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
+    const GoalSearcher & goal_searcher, const bool is_activated,
     const std::optional<PullOverPath> & pull_over_path_opt,
     std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded) const;
 };

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

@@ -0,0 +1,36 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_
+#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_
+
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <vector>
+
+namespace autoware::behavior_path_planner
+{
+struct GoalCandidate
+{
+  geometry_msgs::msg::Pose goal_pose{};
+  double distance_from_original_goal{0.0};
+  double lateral_offset{0.0};
+  size_t id{0};
+  bool is_safe{true};
+  size_t num_objects_to_avoid{0};
+};
+using GoalCandidates = std::vector<GoalCandidate>;
+
+}  // namespace autoware::behavior_path_planner
+#endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_

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

@@ -335,8 +335,8 @@ class GoalPlannerModule : public SceneModuleInterface
   std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
 
   // goal searcher
-  std::shared_ptr<GoalSearcherBase> goal_searcher_;
-  GoalCandidates goal_candidates_{};
+  std::optional<GoalSearcher> goal_searcher_{};
+  std::optional<GoalCandidates> goal_candidates_{};
 
   // NOTE: this is latest occupancy_grid_map pointer which the local planner_data on
   // onFreespaceParkingTimer thread storage may point to while calculation.
@@ -362,7 +362,7 @@ class GoalPlannerModule : public SceneModuleInterface
   mutable GoalPlannerDebugData debug_data_;
 
   // goal seach
-  GoalCandidates generateGoalCandidates() const;
+  GoalCandidates generateGoalCandidates(GoalSearcher & goal_searcher) const;
 
   /*
    * state transitions and plan function used in each state
@@ -431,7 +431,6 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
-    const GoalCandidates & goal_candidates,
     const std::optional<std::vector<size_t>> sorted_bezier_indices_opt) const;
 
   // lanes and drivable area

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

@@ -15,7 +15,12 @@
 #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_
 
-#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_planner_common/data_manager.hpp"
+#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <memory>
 #include <vector>
@@ -24,31 +29,44 @@ namespace autoware::behavior_path_planner
 {
 using autoware::universe_utils::LinearRing2d;
 using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
+using autoware::universe_utils::MultiPolygon2d;
 
-class GoalSearcher : public GoalSearcherBase
+class GoalSearcher
 {
 public:
-  GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
+  static GoalSearcher create(
+    const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
+    const std::shared_ptr<const PlannerData> & planner_data);
 
-  GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
+public:
+  GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data);
   void update(
     GoalCandidates & goal_candidates,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const std::shared_ptr<const PlannerData> & planner_data,
-    const PredictedObjects & objects) const override;
+    const PredictedObjects & objects) const;
 
   // todo(kosuke55): Functions for this specific use should not be in the interface,
   // so it is better to consider interface design when we implement other goal searchers.
   GoalCandidate getClosetGoalCandidateAlongLanes(
     const GoalCandidates & goal_candidates,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data) const;
   bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const std::shared_ptr<const PlannerData> & planner_data,
-    const PredictedObjects & objects) const override;
+    const PredictedObjects & objects) const;
+
+  MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
 
 private:
+  GoalSearcher(
+    const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint,
+    const bool left_side_parking, const lanelet::ConstLanelets & pull_over_lanes,
+    const lanelet::BasicPolygons2d & no_parking_area_polygons,
+    const lanelet::BasicPolygons2d & no_stopping_area_polygons,
+    const lanelet::BasicPolygons2d & bus_stop_area_polygons);
+
   void countObjectsToAvoid(
     GoalCandidates & goal_candidates, const PredictedObjects & objects,
     const std::shared_ptr<const PlannerData> & planner_data,
@@ -63,11 +81,16 @@ class GoalSearcher : public GoalSearcherBase
     const Pose & ego_pose, const PredictedObjects & objects,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
     const std::shared_ptr<const PlannerData> & planner_data) const;
-  BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
-  BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
 
-  LinearRing2d vehicle_footprint_{};
-  bool left_side_parking_{true};
+  const GoalPlannerParameters parameters_;
+  const LinearRing2d vehicle_footprint_;
+  const bool left_side_parking_;
+  const lanelet::ConstLanelets pull_over_lanes_;
+  const lanelet::BasicPolygons2d no_parking_area_polygons_;
+  const lanelet::BasicPolygons2d no_stopping_area_polygons_;
+  const lanelet::BasicPolygons2d bus_stop_area_polygons_;
+
+  MultiPolygon2d area_polygons_{};
 };
 }  // namespace autoware::behavior_path_planner
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp

@@ -14,8 +14,8 @@
 
 #pragma once
 
+#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp"
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
-#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
 #include "autoware/behavior_path_planner_common/data_manager.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>

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

@@ -15,7 +15,7 @@
 #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
 #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
 
-#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp"
 #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp"
 
 #include <autoware/lane_departure_checker/lane_departure_checker.hpp>

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp

@@ -32,7 +32,7 @@ void PathDecisionStateController::transit_state(
   const std::shared_ptr<const PlannerData> planner_data,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
   const bool is_current_safe, const GoalPlannerParameters & parameters,
-  const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
+  const GoalSearcher & goal_searcher, const bool is_activated,
   const std::optional<PullOverPath> & pull_over_path,
   std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded)
 {
@@ -50,7 +50,7 @@ PathDecisionState PathDecisionStateController::get_next_state(
   const std::shared_ptr<const PlannerData> planner_data,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
   const bool is_current_safe, const GoalPlannerParameters & parameters,
-  const std::shared_ptr<GoalSearcherBase> goal_searcher, const bool is_activated,
+  const GoalSearcher & goal_searcher, const bool is_activated,
   const std::optional<PullOverPath> & pull_over_path_opt,
   std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded) const
 {
@@ -105,7 +105,7 @@ PathDecisionState PathDecisionStateController::get_next_state(
 
     // check goal pose collision
     if (
-      modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor(
+      modified_goal_opt && !goal_searcher.isSafeGoalWithMarginScaleFactor(
                              modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map,
                              planner_data, static_target_objects)) {
       RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe");