From 96f169bc723741274529a1dc15c865e7feae50ac Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 5 Oct 2023 15:08:15 +0900 Subject: [PATCH] refactor(safety_check): define filtering function in safety check library Signed-off-by: satoshi-ota --- .../path_safety_checker/objects_filtering.hpp | 18 ++++++++++++++ .../goal_planner/goal_planner_module.cpp | 8 ++----- .../lane_change/avoidance_by_lane_change.cpp | 8 ++----- .../start_planner/start_planner_module.cpp | 8 ++----- .../src/utils/avoidance/utils.cpp | 24 +++++++++---------- .../src/utils/goal_planner/goal_searcher.cpp | 8 ++----- .../path_safety_checker/objects_filtering.cpp | 14 +++++++++++ .../start_planner/geometric_pull_out.cpp | 8 ++----- .../utils/start_planner/shift_pull_out.cpp | 7 +----- 9 files changed, 55 insertions(+), 48 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 4d5b9052a4681..15b15bac40b51 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 @@ -38,6 +38,24 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters objects based on object centroid position. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + +/** + * @brief Filters objects based on object polygon overlapping with lanelet. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + /** * @brief Filters objects based on various criteria. * 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 4de8720a4a52d..9c2544cf60cec 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 @@ -1301,17 +1301,13 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return false; } - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; 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); const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes, condition); + *(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); const auto common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index ce49501b78d08..1f305e3988828 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -141,14 +141,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets, condition); + *planner_data_->dynamic_object, data.current_lanelets, + utils::path_safety_checker::isPolygonOverlapLanelet); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for 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 f19bcf30e7771..c828a747856d8 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 @@ -771,14 +771,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses( std::vector pull_out_start_pose{}; // filter pull out lanes stop objects - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes, condition); + *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); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d7da362402197..0e7eb87feed9b 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1529,12 +1529,6 @@ std::vector getSafetyCheckTargetObjects( return ret; }; - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); - }; - const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1551,13 +1545,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } @@ -1568,13 +1564,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } @@ -1585,13 +1583,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } 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 bd374c12427bc..12044980ebd81 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,18 +266,14 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), 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); const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes, condition); + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); 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 e63d35f7bc925..dfc9f76b25e33 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 @@ -24,6 +24,20 @@ namespace behavior_path_planner::utils::path_safety_checker { +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); +} + +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); +} + PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, 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 f4bfb4d681440..3cf42dbb5da26 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,15 +63,11 @@ 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 condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes, condition); + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, 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 02c6d41bdbf08..a430e18c330e1 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 @@ -65,14 +65,9 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P } // extract stop objects in pull out lane for collision check - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *dynamic_objects, pull_out_lanes, condition); + *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);