From 0ece800f20484ef18ad73f0230fc68992b532599 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Mon, 16 Oct 2023 13:17:49 +0900 Subject: [PATCH] standardize the return type by using void Signed-off-by: Zulfaqar Azmi --- .../path_safety_checker/objects_filtering.hpp | 56 +++++++++++++++---- .../goal_planner/goal_planner_module.cpp | 5 +- .../start_planner/start_planner_module.cpp | 6 +- .../src/utils/goal_planner/goal_searcher.cpp | 5 +- .../path_safety_checker/objects_filtering.cpp | 44 ++++----------- .../start_planner/geometric_pull_out.cpp | 5 +- .../utils/start_planner/shift_pull_out.cpp | 5 +- 7 files changed, 74 insertions(+), 52 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 d39501a5d565c..171ce187adbbf 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 @@ -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 +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 +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_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index c6e68f8e81af7..2685f4e2dd3d8 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/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; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c828a747856d8..a9ac428e34b64 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -775,8 +775,10 @@ std::vector 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. diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 12044980ebd81..7261550d78c9a 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/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); 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 567b0c918b33a..2cc7f8f133da9 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 @@ -23,21 +23,6 @@ namespace behavior_path_planner::utils::path_safety_checker { - -template -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 & 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,19 +69,18 @@ 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::max()); + filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::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( @@ -104,7 +88,7 @@ PredictedObjects filterObjectsByVelocity( 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> separateObjectIndicesByLanelets( diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 3cf42dbb5da26..c4037217b7c4e 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -63,8 +63,9 @@ boost::optional 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); diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index a430e18c330e1..9e857aae3eadd 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -68,8 +68,9 @@ boost::optional 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) {