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 e652dde998b39..356f30f6d5842 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 @@ -150,17 +150,7 @@ bool StartPlannerModule::isExecutionRequested() const /*forward_only_in_route*/ true); const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } + const auto lanes = utils::combineLanelets(current_lanes, pull_out_lanes); if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { return false; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 7760b9b57d8ab..5594f7af1c428 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -47,13 +47,12 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +61,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); if (!found_valid_path) { return {}; } 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 f4f65474d4a30..628f1cfd7e421 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 @@ -47,16 +47,6 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);