standardize the return type by using void

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

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

@@ -79,15 +79,14 @@ PredictedObjects filterObjects(
  * velocities above the given threshold or only keeps those objects. It uses the helper function
  * filterObjectsByVelocity() to do the actual filtering.
  *
- * @param objects The objects to be filtered.
+ * @param objects [in, out] The objects to be filtered.
  * @param velocity_threshold The velocity threshold for the filtering.
  * @param remove_above_threshold If true, objects with velocities above the threshold are removed.
  *                               If false, only objects with velocities above the threshold are
  * kept.
- * @return A new collection of objects that have been filtered according to the rules.
  */
-PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, const double velocity_threshold,
+void filterObjectsByVelocity(
+  PredictedObjects & objects, const double velocity_threshold,
   const bool remove_above_threshold = true);
 
 /**
@@ -97,18 +96,17 @@ PredictedObjects filterObjectsByVelocity(
  * is within the velocity_threshold and max_velocity range, the object is added to a new collection.
  * This new collection is then returned.
  *
- * @param objects The objects to be filtered.
+ * @param objects [in, out] The objects to be filtered.
  * @param velocity_threshold The minimum velocity for an object to be included in the output.
  * @param max_velocity The maximum velocity for an object to be included in the output.
- * @return A new collection of objects that have been filtered according to the rules.
  */
-PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, double velocity_threshold, double max_velocity);
+void filterObjectsByVelocity(
+  PredictedObjects & objects, double velocity_threshold, double max_velocity);
 
 /**
  * @brief Filter objects based on their position relative to a current_pose.
  *
- * @param objects The predicted objects to filter.
+ * @param objects [in, out] The predicted objects to filter.
  * @param path_points Points on the path.
  * @param current_pose Current pose of the reference (e.g., ego vehicle).
  * @param forward_distance Maximum forward distance for filtering.
@@ -122,7 +120,7 @@ void filterObjectsByPosition(
 /**
  * @brief Filters the provided objects based on their classification.
  *
- * @param objects The predicted objects to be filtered.
+ * @param objects [in, out] The predicted objects to be filtered.
  * @param target_object_types The types of objects to retain after filtering.
  */
 void filterObjectsByClass(
@@ -233,6 +231,44 @@ TargetObjectsOnLane createTargetObjectsOnLane(
  */
 bool isTargetObjectType(
   const PredictedObject & object, const ObjectTypesToCheck & target_object_types);
+
+/**
+ * @brief Filters objects in the 'selected' container based on the provided filter function.
+ *
+ * This function partitions the 'selected' container based on the 'filter' function
+ * and moves objects that satisfy the filter condition to the 'removed' container.
+ *
+ * @tparam Func The type of the filter function.
+ * @param selected [in,out] The container of objects to be filtered.
+ * @param removed [out] The container where objects not satisfying the filter condition are moved.
+ * @param filter The filter function that determines whether an object should be removed.
+ */
+template <typename Func>
+void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func filter)
+{
+  auto partitioned = std::partition(selected.objects.begin(), selected.objects.end(), filter);
+  removed.objects.insert(removed.objects.end(), partitioned, selected.objects.end());
+  selected.objects.erase(partitioned, selected.objects.end());
+}
+
+/**
+ * @brief Filters objects in the 'objects' container based on the provided filter function.
+ *
+ * This function is an overload that simplifies filtering when you don't need to specify a separate
+ * 'removed' container. It internally creates a 'removed_objects' container and calls the main
+ * 'filterObjects' function.
+ *
+ * @tparam Func The type of the filter function.
+ * @param objects [in,out] The container of objects to be filtered.
+ * @param filter The filter function that determines whether an object should be removed.
+ */
+template <typename Func>
+void filterObjects(PredictedObjects & objects, Func filter)
+{
+  [[maybe_unused]] PredictedObjects removed_objects{};
+  filterObjects(objects, removed_objects, filter);
+}
+
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_

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

@@ -1316,8 +1316,9 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
     utils::path_safety_checker::separateObjectsByLanelets(
       *(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);
+  auto pull_over_lane_stop_objects = pull_over_lane_objects;
+  utils::path_safety_checker::filterObjectsByVelocity(
+    pull_over_lane_stop_objects, parameters_->th_moving_object_velocity);
   const auto common_parameters = planner_data_->parameters;
   const double base_link2front = common_parameters.base_link2front;
   const double base_link2rear = common_parameters.base_link2rear;

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

@@ -775,8 +775,10 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
     utils::path_safety_checker::separateObjectsByLanelets(
       *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);
+
+  auto pull_out_lane_stop_objects = pull_out_lane_objects;
+  utils::path_safety_checker::filterObjectsByVelocity(
+    pull_out_lane_stop_objects, parameters_->th_moving_object_velocity);
 
   // Set the maximum backward distance less than the distance from the vehicle's base_link to the
   // lane's rearmost point to prevent lane departure.

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

@@ -266,8 +266,9 @@ 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);
+  auto stop_objects = *(planner_data_->dynamic_object);
+  utils::path_safety_checker::filterObjectsByVelocity(
+    stop_objects, 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);

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

@@ -23,21 +23,6 @@
 
 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;
-}
-
 bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet)
 {
   const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position;
@@ -59,7 +44,7 @@ PredictedObjects filterObjects(
   const std::shared_ptr<ObjectsFilteringParams> & params)
 {
   // Guard
-  if (objects->objects.empty()) {
+  if (!objects || objects->objects.empty()) {
     return PredictedObjects();
   }
 
@@ -68,9 +53,9 @@ PredictedObjects filterObjects(
   const double object_check_backward_distance = params->object_check_backward_distance;
   const ObjectTypesToCheck & target_object_types = params->object_types_to_check;
 
-  PredictedObjects filtered_objects;
+  auto filtered_objects = *objects;
 
-  filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false);
+  filterObjectsByVelocity(filtered_objects, ignore_object_velocity_threshold, false);
 
   filterObjectsByClass(filtered_objects, target_object_types);
 
@@ -84,27 +69,26 @@ PredictedObjects filterObjects(
   return filtered_objects;
 }
 
-PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, const double velocity_threshold,
-  const bool remove_above_threshold)
+void filterObjectsByVelocity(
+  PredictedObjects & objects, const double velocity_threshold, const bool remove_above_threshold)
 {
   if (remove_above_threshold) {
-    return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold);
+    filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold);
   }
 
-  return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
+  filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
 }
 
-PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, double velocity_threshold, double max_velocity)
+void filterObjectsByVelocity(
+  PredictedObjects & objects, double velocity_threshold, double max_velocity)
 {
   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);
     return velocity_threshold < v_norm && v_norm < max_velocity;
   };
-  return filterObjects(objects, velocity_filter);
+  filterObjects(objects, velocity_filter);
 }
 
 void filterObjectsByPosition(
@@ -120,9 +104,7 @@ void filterObjectsByPosition(
   };
 
   // Erase objects based on filtered list
-  objects.objects.erase(
-    std::partition(objects.objects.begin(), objects.objects.end(), position_filter),
-    objects.objects.end());
+  filterObjects(objects, position_filter);
 }
 
 void filterObjectsByClass(
@@ -133,9 +115,7 @@ void filterObjectsByClass(
   };
 
   // Erase objects based on filtered list
-  objects.objects.erase(
-    std::partition(objects.objects.begin(), objects.objects.end(), object_class_filter),
-    objects.objects.end());
+  filterObjects(objects, object_class_filter);
 }
 
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(

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

@@ -63,8 +63,9 @@ 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 & stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
-    *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity);
+  auto stop_objects = *(planner_data_->dynamic_object);
+  utils::path_safety_checker::filterObjectsByVelocity(
+    stop_objects, parameters_.th_moving_object_velocity);
   const auto [pull_out_lane_stop_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
       stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);

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

@@ -68,8 +68,9 @@ boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const P
   const auto [pull_out_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
       *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);
+  auto pull_out_lane_stop_objects = pull_out_lane_objects;
+  utils::path_safety_checker::filterObjectsByVelocity(
+    pull_out_lane_stop_objects, parameters_.th_moving_object_velocity);
 
   // get safe path
   for (auto & pull_out_path : pull_out_paths) {