test(bpp_common): add test for objects filtering except for lanelet using functions (#9049)

* add test for objects filtering except for lanelet using functions

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* remove unnecessary include file

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* add doxygen

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

---------

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

Author: go-sakayori

planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt

@@ -60,6 +60,7 @@ if(BUILD_TESTING)
 
   ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check
     test/test_safety_check.cpp
+    test/test_objects_filtering.cpp
   )
 
   target_link_libraries(test_${PROJECT_NAME}_safety_check

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp

@@ -34,14 +34,44 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker::filter
 using autoware_perception_msgs::msg::PredictedObject;
 using tier4_planning_msgs::msg::PathPointWithLaneId;
 
+/**
+ * @brief Filters object based on velocity.
+ *
+ * @param object The predicted object to filter.
+ * @param velocity_threshold Lower bound
+ * @param max_velocity Upper bound
+ * @return Returns true when the object is within a certain velocity range.
+ */
 bool velocity_filter(
   const PredictedObject & object, double velocity_threshold, double max_velocity);
 
+/**
+ * @brief Filters object based on position.
+ *
+ * @param object The predicted object to filter.
+ * @param path_points Ego path
+ * @param current_pose Ego current pose
+ * @param forward_distance Upper bound for relative distance
+ * @param backward_distance Lower bound for relative distance
+ * @return Returns true when the object is within a certain distance.
+ */
 bool position_filter(
-  PredictedObject & object, const std::vector<PathPointWithLaneId> & path_points,
+  const PredictedObject & object, const std::vector<PathPointWithLaneId> & path_points,
   const geometry_msgs::msg::Point & current_pose, const double forward_distance,
   const double backward_distance);
 
+/**
+ * @brief Filters object by searching within a radius.
+ *
+ * @param object The predicted object to filter.
+ * @param reference_point Reference point
+ * @param search_radius Search radius
+ * @return Returns true when the object is within radius.
+ */
+bool is_within_circle(
+  const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point,
+  const double search_radius);
+
 }  // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker

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

@@ -39,6 +39,8 @@ bool position_filter(
   const geometry_msgs::msg::Point & current_pose, const double forward_distance,
   const double backward_distance)
 {
+  if (path_points.empty()) return false;
+
   const auto dist_ego_to_obj = autoware::motion_utils::calcSignedArcLength(
     path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position);