From f42f760620a88b8008ae478d3143accff1576184 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 25 Aug 2023 23:14:18 +0900
Subject: [PATCH] feat(behavior_path_planner): update filter objects by
 velocity (#4742)

---
 .../path_safety_checker/objects_filtering.hpp | 35 +++++++++++++------
 .../path_safety_checker/objects_filtering.cpp | 16 ++++++---
 2 files changed, 35 insertions(+), 16 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp
index b4575eb4d3b7e..8a3b7094c69d1 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp
@@ -55,24 +55,37 @@ PredictedObjects filterObjects(
   const std::shared_ptr<ObjectsFilteringParams> & params);
 
 /**
- * @brief Filter objects based on their velocity.
+ * @brief Filters objects based on their velocity.
  *
- * @param objects The predicted objects to filter.
- * @param lim_v Velocity limit for filtering.
- * @return PredictedObjects The filtered objects.
+ * Depending on the remove_above_threshold parameter, this function either removes objects with
+ * 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 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, double lim_v);
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold = true);
 
 /**
- * @brief Filter objects based on a velocity range.
+ * @brief Helper function to filter objects based on their velocity.
  *
- * @param objects The predicted objects to filter.
- * @param min_v Minimum velocity for filtering.
- * @param max_v Maximum velocity for filtering.
- * @return PredictedObjects The filtered objects.
+ * This function iterates over all objects and calculates their velocity norm. If the velocity norm
+ * 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 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 min_v, double max_v);
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity);
 
 /**
  * @brief Filter objects based on their position relative to a current_pose.
diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
index 33afe0fe6642f..a775e7c16efed 100644
--- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
+++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp
@@ -37,7 +37,7 @@ PredictedObjects filterObjects(
 
   PredictedObjects filtered_objects;
 
-  filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold);
+  filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false);
 
   filterObjectsByClass(filtered_objects, target_object_types);
 
@@ -51,13 +51,19 @@ PredictedObjects filterObjects(
   return filtered_objects;
 }
 
-PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v)
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold)
 {
-  return filterObjectsByVelocity(objects, -lim_v, lim_v);
+  if (remove_above_threshold) {
+    return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold);
+  } else {
+    return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits<double>::max());
+  }
 }
 
 PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, double min_v, double max_v)
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity)
 {
   PredictedObjects filtered;
   filtered.header = objects.header;
@@ -65,7 +71,7 @@ PredictedObjects filterObjectsByVelocity(
     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 (min_v < v_norm && v_norm < max_v) {
+    if (velocity_threshold < v_norm && v_norm < max_velocity) {
       filtered.objects.push_back(obj);
     }
   }