Merge branch 'awf-latest' into feat/traffic_light-v2i

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/config/goal_planner.param.yaml

@@ -39,7 +39,6 @@
 
       # object recognition
       object_recognition:
-        use_object_recognition: true
         collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]  # the maximum margin when ego and objects are oriented.
         collision_check_hard_margins: [0.6]  # this should be larger than `surround_check_distance` of surround_obstacle_checker
         object_recognition_collision_check_max_extra_stopping_margin: 1.0

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

@@ -335,7 +335,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};
@@ -364,7 +364,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 +434,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_planner_parameters.hpp

@@ -77,7 +77,6 @@ struct GoalPlannerParameters
   int obstacle_threshold{0};
 
   // object recognition
-  bool use_object_recognition{false};
   std::vector<double> object_recognition_collision_check_soft_margins{};
   std::vector<double> object_recognition_collision_check_hard_margins{};
   double object_recognition_collision_check_max_extra_stopping_margin{0.0};

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(
     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/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>