diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 294a5125a0d13..be56d9dc30196 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset); +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh); double calcLateralDeviationBetweenPaths( const PathWithLaneId & reference_path, const PathWithLaneId & target_path); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index fa6a44b97a623..2d3133768154e 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1509,22 +1509,12 @@ bool GoalPlannerModule::checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, const bool update_debug_data) const { - 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 = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, parameters_->detection_bound_offset, + *(planner_data_->dynamic_object), parameters_->th_moving_object_velocity); - const auto expanded_pull_over_lanes = - left_side_parking_ ? lanelet::utils::getExpandedLanelets( - pull_over_lanes, parameters_->detection_bound_offset, 0.0) - : lanelet::utils::getExpandedLanelets( - pull_over_lanes, 0.0, parameters_->detection_bound_offset); - - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), expanded_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); std::vector obj_polygons; for (const auto & object : pull_over_lane_stop_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index 31f99e8ea5aa1..fd795177eb0b7 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -266,14 +266,11 @@ 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); - 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, utils::path_safety_checker::isPolygonOverlapLanelet); + const auto pull_over_lane_stop_objects = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, parameters_.detection_bound_offset, + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index dfdaa2a3a18c1..8ad90d26f58f2 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -41,6 +42,7 @@ using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; + lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance) @@ -74,6 +76,41 @@ lanelet::ConstLanelets getPullOverLanes( outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset) +{ + const auto pull_over_lanes = + getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); + + return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) + : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset); +} + +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects) +{ + const auto lanes = generateExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset); + + const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( + objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + return objects_in_lanes; +} + +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh) +{ + const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset, objects); + + return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh); +} + PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside)