refactor(behavior_path_planner): use helper function for filters

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

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

@@ -213,7 +213,6 @@ TargetObjectsOnLane createTargetObjectsOnLane(
  */
 bool isTargetObjectType(
   const PredictedObject & object, const ObjectTypesToCheck & target_object_types);
-
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_

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

@@ -24,6 +24,20 @@
 namespace behavior_path_planner::utils::path_safety_checker
 {
 
+template <typename Func>
+PredictedObjects filterObjects(const PredictedObjects & objects, Func filter)
+{
+  PredictedObjects filtered;
+  filtered.header = objects.header;
+  filtered.objects.reserve(objects.objects.size());
+  for (const auto & obj : objects.objects) {
+    if (filter(obj)) {
+      filtered.objects.push_back(obj);
+    }
+  }
+  return filtered;
+}
+
 PredictedObjects filterObjects(
   const std::shared_ptr<const PredictedObjects> & objects,
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
@@ -62,66 +76,50 @@ PredictedObjects filterObjectsByVelocity(
 {
   if (remove_above_threshold) {
     return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold);
-  } else {
-    return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
   }
+
+  return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
 }
 
 PredictedObjects filterObjectsByVelocity(
   const PredictedObjects & objects, double velocity_threshold, double max_velocity)
 {
-  PredictedObjects filtered;
-  filtered.header = objects.header;
-  for (const auto & obj : objects.objects) {
+  const auto velocity_filter = [velocity_threshold, max_velocity](const auto & obj) {
     const auto v_norm = std::hypot(
       obj.kinematics.initial_twist_with_covariance.twist.linear.x,
       obj.kinematics.initial_twist_with_covariance.twist.linear.y);
-    if (velocity_threshold < v_norm && v_norm < max_velocity) {
-      filtered.objects.push_back(obj);
-    }
-  }
-  return filtered;
+    return velocity_threshold < v_norm && v_norm < max_velocity;
+  };
+  return filterObjects(objects, velocity_filter);
 }
 
 void filterObjectsByPosition(
   PredictedObjects & objects, const std::vector<PathPointWithLaneId> & path_points,
   const geometry_msgs::msg::Point & current_pose, const double forward_distance,
   const double backward_distance)
 {
-  // Create a new container to hold the filtered objects
-  PredictedObjects filtered;
-  filtered.header = objects.header;
-
-  // Reserve space in the vector to avoid reallocations
-  filtered.objects.reserve(objects.objects.size());
-
-  for (const auto & obj : objects.objects) {
+  auto position_filter = [&path_points, &current_pose, forward_distance,
+                          backward_distance](const auto & obj) {
     const double dist_ego_to_obj = motion_utils::calcSignedArcLength(
       path_points, current_pose, obj.kinematics.initial_pose_with_covariance.pose.position);
+    return (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance);
+  };
 
-    if (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance) {
-      filtered.objects.push_back(obj);
-    }
-  }
+  // Create a new container to hold the filtered objects
+  PredictedObjects filtered = filterObjects(objects, position_filter);
 
   // Replace the original objects with the filtered list
   objects.objects = std::move(filtered.objects);
-  return;
 }
 
 void filterObjectsByClass(
   PredictedObjects & objects, const ObjectTypesToCheck & target_object_types)
 {
-  PredictedObjects filtered_objects;
-
-  for (auto & object : objects.objects) {
-    const auto is_object_type = isTargetObjectType(object, target_object_types);
+  const auto object_class_filter = [&target_object_types](const auto & object) {
+    return isTargetObjectType(object, target_object_types);
+  };
 
-    // If the object type matches any of the target types, add it to the filtered list
-    if (is_object_type) {
-      filtered_objects.objects.push_back(object);
-    }
-  }
+  PredictedObjects filtered_objects = filterObjects(objects, object_class_filter);
 
   // Replace the original objects with the filtered list
   objects = std::move(filtered_objects);