test(bpp_common): add test for objects filtering except for lanelet u…

…sing functions (autowarefoundation#9049) * add test for objects filtering except for lanelet using functions Signed-off-by: Go Sakayori <[email protected]> * remove unnecessary include file Signed-off-by: Go Sakayori <[email protected]> * add doxygen Signed-off-by: Go Sakayori <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]>
technolojin · Oct 16, 2024 · a3c7ede · a3c7ede
1 parent 7cf1ffa
commit a3c7ede
Show file tree

Hide file tree

Showing 4 changed files with 416 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/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

diff --git a/...ude/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/...ude/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

diff --git a/...autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/...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);