feat(goal_planner): use expanded pull over lanes for filtering in goa…

…l searcher (#6142)

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp

@@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray;
 lanelet::ConstLanelets getPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance);
+lanelet::ConstLanelets generateExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset);
+PredictedObjects extractObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects);
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside);
+PredictedObjects extractStaticObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects,
+  const double velocity_thresh);
 
 double calcLateralDeviationBetweenPaths(
   const PathWithLaneId & reference_path, const PathWithLaneId & target_path);

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1509,22 +1509,12 @@ bool GoalPlannerModule::checkObjectsCollision(
   const PathWithLaneId & path, const double collision_check_margin,
   const bool update_debug_data) const
 {
-  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 pull_over_lane_stop_objects =
+    goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
+      *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
+      parameters_->forward_goal_search_length, parameters_->detection_bound_offset,
+      *(planner_data_->dynamic_object), parameters_->th_moving_object_velocity);
 
-  const auto expanded_pull_over_lanes =
-    left_side_parking_ ? lanelet::utils::getExpandedLanelets(
-                           pull_over_lanes, parameters_->detection_bound_offset, 0.0)
-                       : lanelet::utils::getExpandedLanelets(
-                           pull_over_lanes, 0.0, parameters_->detection_bound_offset);
-
-  const auto [pull_over_lane_objects, others] =
-    utils::path_safety_checker::separateObjectsByLanelets(
-      *(planner_data_->dynamic_object), expanded_pull_over_lanes,
-      utils::path_safety_checker::isPolygonOverlapLanelet);
-  const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
-    pull_over_lane_objects, parameters_->th_moving_object_velocity);
   std::vector<Polygon2d> obj_polygons;
   for (const auto & object : pull_over_lane_stop_objects.objects) {
     obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object));

planning/behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -266,14 +266,11 @@ void GoalSearcher::countObjectsToAvoid(
 
 void GoalSearcher::update(GoalCandidates & goal_candidates) const
 {
-  const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
-    *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity);
-  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 [pull_over_lane_stop_objects, others] =
-    utils::path_safety_checker::separateObjectsByLanelets(
-      stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+  const auto pull_over_lane_stop_objects =
+    goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
+      *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
+      parameters_.forward_goal_search_length, parameters_.detection_bound_offset,
+      *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity);
 
   if (parameters_.prioritize_goals_before_objects) {
     countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects);

planning/behavior_path_goal_planner_module/src/util.cpp

@@ -14,6 +14,7 @@
 
 #include "behavior_path_goal_planner_module/util.hpp"
 
+#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
@@ -41,6 +42,7 @@ using tier4_autoware_utils::createDefaultMarker;
 using tier4_autoware_utils::createMarkerColor;
 using tier4_autoware_utils::createMarkerScale;
 using tier4_autoware_utils::createPoint;
+
 lanelet::ConstLanelets getPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance)
@@ -74,6 +76,41 @@ lanelet::ConstLanelets getPullOverLanes(
     outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes);
 }
 
+lanelet::ConstLanelets generateExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset)
+{
+  const auto pull_over_lanes =
+    getPullOverLanes(route_handler, left_side, backward_distance, forward_distance);
+
+  return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0)
+                   : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset);
+}
+
+PredictedObjects extractObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects)
+{
+  const auto lanes = generateExpandedPullOverLanes(
+    route_handler, left_side, backward_distance, forward_distance, bound_offset);
+
+  const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
+    objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+
+  return objects_in_lanes;
+}
+
+PredictedObjects extractStaticObjectsInExpandedPullOverLanes(
+  const RouteHandler & route_handler, const bool left_side, const double backward_distance,
+  const double forward_distance, double bound_offset, const PredictedObjects & objects,
+  const double velocity_thresh)
+{
+  const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes(
+    route_handler, left_side, backward_distance, forward_distance, bound_offset, objects);
+
+  return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh);
+}
+
 PredictedObjects filterObjectsByLateralDistance(
   const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
   const double distance_thresh, const bool filter_inside)