Merge pull request #1805 from tier4/cherry-pick/goal-planner

feat(goal_planner): cherry pick several fixes

Author: TomohitoAndo

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md

@@ -384,7 +384,6 @@ In addition, the safety check has a time hysteresis, and if the path is judged "
 
 | Name                                 | Unit  | Type   | Description                                                                                              | Default value                |
 | :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- |
-| enable_safety_check                  | [-]   | bool   | flag whether to use safety check                                                                         | true                         |
 | method                               | [-]   | string | method for safety check. `RSS` or `integral_predicted_polygon`                                           | `integral_predicted_polygon` |
 | keep_unsafe_time                     | [s]   | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0                          |
 | check_all_predicted_path             | -     | bool   | Flag to check all predicted paths                                                                        | true                         |

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"
 
@@ -49,15 +49,13 @@ class PathDecisionStateController
    * @brief update current state and save old current state to prev state
    */
   void transit_state(
-    const bool found_pull_over_path, const rclcpp::Time & now,
+    const std::optional<PullOverPath> & pull_over_path_opt, const rclcpp::Time & now,
     const autoware_perception_msgs::msg::PredictedObjects & static_target_objects,
     const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects,
-    const std::optional<GoalCandidate> modified_goal_opt,
     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 std::optional<PullOverPath> & pull_over_path,
+    const GoalSearcher & goal_searcher,
     std::vector<autoware::universe_utils::Polygon2d> & ego_polygons_expanded);
 
   PathDecisionState get_current_state() const { return current_state_; }
@@ -71,14 +69,12 @@ class PathDecisionStateController
    * @brief update current state and save old current state to prev state
    */
   PathDecisionState get_next_state(
-    const bool found_pull_over_path, const rclcpp::Time & now,
+    const std::optional<PullOverPath> & pull_over_path_opt, const rclcpp::Time & now,
     const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
-    const std::optional<GoalCandidate> modified_goal_opt,
     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 std::optional<PullOverPath> & pull_over_path_opt,
+    const GoalSearcher & goal_searcher,
     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

@@ -73,14 +73,6 @@ struct GoalPlannerDebugData
   utils::path_safety_checker::CollisionCheckDebugMap collision_check{};
 };
 
-struct LastApprovalData
-{
-  LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {}
-
-  rclcpp::Time time{};
-  Pose pose{};
-};
-
 struct PullOverContextData
 {
   PullOverContextData() = delete;
@@ -335,7 +327,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
 
   // goal searcher
-  std::shared_ptr<GoalSearcherBase> goal_searcher_;
+  std::optional<GoalSearcher> goal_searcher_{};
   GoalCandidates goal_candidates_{};
 
   bool use_bus_stop_area_{false};
@@ -352,7 +344,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<PullOverContextData> context_data_{std::nullopt};
   // path_decision_controller is updated in updateData(), and used in plan()
   PathDecisionStateController path_decision_controller_{getLogger()};
-  std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
+  std::optional<rclcpp::Time> decided_time_{};
 
   // approximate distance from the start point to the end point of pull_over.
   // this is used as an assumed value to decelerate, etc., before generating the actual path.
@@ -364,7 +356,8 @@ class GoalPlannerModule : public SceneModuleInterface
   mutable GoalPlannerDebugData debug_data_;
 
   // goal seach
-  GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) const;
+  GoalCandidates generateGoalCandidates(
+    GoalSearcher & goal_searcher, const bool use_bus_stop_area) const;
 
   /*
    * state transitions and plan function used in each state
@@ -433,7 +426,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,32 +29,45 @@ 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);
 
+public:
   GoalCandidates search(
-    const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area) override;
+    const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area);
   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(
+  std::optional<GoalCandidate> getClosestGoalCandidateAlongLanes(
     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,
@@ -64,11 +82,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/goal_searcher_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 <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>

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

@@ -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>