refactor(safety_check): define filtering function in safety check lib…

…rary (#5228)

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp

@@ -38,6 +38,24 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 
+/**
+ * @brief Filters objects based on object centroid position.
+ *
+ * @param objects The predicted objects to filter.
+ * @param lanelet
+ * @return result.
+ */
+bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet);
+
+/**
+ * @brief Filters objects based on object polygon overlapping with lanelet.
+ *
+ * @param objects The predicted objects to filter.
+ * @param lanelet
+ * @return result.
+ */
+bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet);
+
 /**
  * @brief Filters objects based on various criteria.
  *

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -1301,17 +1301,13 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
     return false;
   }
 
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
-    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
-  };
   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_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *(planner_data_->dynamic_object), pull_over_lanes, condition);
+      *(planner_data_->dynamic_object), 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);
   const auto common_parameters = planner_data_->parameters;

planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp

@@ -141,14 +141,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
 {
   const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);
 
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
-    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
-  };
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *planner_data_->dynamic_object, data.current_lanelets, condition);
+      *planner_data_->dynamic_object, data.current_lanelets,
+      utils::path_safety_checker::isPolygonOverlapLanelet);
 
   // Assume that the maximum allocation for data.other object is the sum of
   // objects_within_target_lane and object_outside_target_lane. The maximum allocation for

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -771,14 +771,10 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
   std::vector<Pose> pull_out_start_pose{};
 
   // filter pull out lanes stop objects
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
-    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
-  };
   const auto [pull_out_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *planner_data_->dynamic_object, status_.pull_out_lanes, condition);
+      *planner_data_->dynamic_object, status_.pull_out_lanes,
+      utils::path_safety_checker::isPolygonOverlapLanelet);
   const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_out_lane_objects, parameters_->th_moving_object_velocity);
 

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -1529,12 +1529,6 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
     return ret;
   };
 
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
-    lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y);
-    return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon());
-  };
-
   const auto unavoidable_objects = [&data]() {
     ObjectDataArray ret;
     std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) {
@@ -1551,13 +1545,15 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
     if (p->check_other_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(data.other_objects), check_lanes, condition);
+        to_predicted_objects(data.other_objects), check_lanes,
+        utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
 
     if (p->check_unavoidable_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(unavoidable_objects), check_lanes, condition);
+        to_predicted_objects(unavoidable_objects), check_lanes,
+        utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
   }
@@ -1568,13 +1564,15 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
     if (p->check_other_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(data.other_objects), check_lanes, condition);
+        to_predicted_objects(data.other_objects), check_lanes,
+        utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
 
     if (p->check_unavoidable_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(unavoidable_objects), check_lanes, condition);
+        to_predicted_objects(unavoidable_objects), check_lanes,
+        utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
   }
@@ -1585,13 +1583,15 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
     if (p->check_other_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(data.other_objects), check_lanes, condition);
+        to_predicted_objects(data.other_objects), check_lanes,
+        utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
 
     if (p->check_unavoidable_object) {
       const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets(
-        to_predicted_objects(unavoidable_objects), check_lanes, condition);
+        to_predicted_objects(unavoidable_objects), check_lanes,
+        utils::path_safety_checker::isCentroidWithinLanelet);
       append(targets);
     }
   }

planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

@@ -266,18 +266,14 @@ void GoalSearcher::countObjectsToAvoid(
 
 void GoalSearcher::update(GoalCandidates & goal_candidates) const
 {
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
-    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
-  };
   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, condition);
+    utils::path_safety_checker::separateObjectsByLanelets(
+      stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
 
   if (parameters_.prioritize_goals_before_objects) {
     countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects);

planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp

@@ -24,6 +24,20 @@
 namespace behavior_path_planner::utils::path_safety_checker
 {
 
+bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet)
+{
+  const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
+  lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y);
+  return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon());
+}
+
+bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet)
+{
+  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
+  const auto lanelet_polygon = utils::toPolygon2d(lanelet);
+  return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
+}
+
 PredictedObjects filterObjects(
   const std::shared_ptr<const PredictedObjects> & objects,
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,

planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp

@@ -63,15 +63,11 @@ boost::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, con
 
   // collision check with stop objects in pull out lanes
   const auto arc_path = planner_.getArcPath();
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
-    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
-  };
   const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity);
   const auto [pull_out_lane_stop_objects, others] =
-    utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes, condition);
+    utils::path_safety_checker::separateObjectsByLanelets(
+      stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
 
   if (utils::checkCollisionBetweenPathFootprintsAndObjects(
         vehicle_footprint_, arc_path, pull_out_lane_stop_objects,

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -65,14 +65,9 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
   }
 
   // extract stop objects in pull out lane for collision check
-  const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
-    const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
-    const auto lanelet_polygon = utils::toPolygon2d(lanelet);
-    return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
-  };
   const auto [pull_out_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *dynamic_objects, pull_out_lanes, condition);
+      *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
   const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_out_lane_objects, parameters_.th_moving_object_velocity);