refactor(start/goal_planner): use combineLanelets (#4894)

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Sep 5, 2023 · b1f1548 · b1f1548
1 parent c03ca78
commit b1f1548
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 26 deletions.
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
@@ -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
@@ -47,13 +47,12 @@ boost::optional<PullOverPath> 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<PullOverPath> 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
@@ -47,16 +47,6 @@ boost::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, con
     planner_data_, backward_path_length, std::numeric_limits<double>::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);