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 9aba6a05e89d7..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,25 +55,37 @@ PredictedObjects filterObjects( const std::shared_ptr & 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, const double lim_v, const bool filter_dynamic_objects = true); + 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 fee8d51db5df0..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 @@ -52,17 +52,18 @@ PredictedObjects filterObjects( } PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double lim_v, const bool filter_dynamic_objects) + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold) { - if (filter_dynamic_objects) { - return filterObjectsByVelocity(objects, -lim_v, lim_v); + if (remove_above_threshold) { + return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); } else { - return filterObjectsByVelocity(objects, lim_v, std::numeric_limits::max()); + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::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; @@ -70,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); } }