refactor(lane_change): create new helper functions to clean code (#4495)

autowarefoundation · Aug 2, 2023 · 7e893e8 · 7e893e8
1 parent 592a6b2
commit 7e893e8
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 15 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
@@ -76,8 +76,8 @@ std::vector<int64_t> replaceWithSortedIds(
   const std::vector<std::vector<int64_t>> & sorted_lane_ids);
 
 std::vector<std::vector<int64_t>> getSortedLaneIds(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes, const double rough_shift_length);
+  const RouteHandler & route_handler, const Pose & current_pose,
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes);
 
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
 
@@ -90,6 +90,10 @@ lanelet::ConstLanelets getTargetNeighborLanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const LaneChangeModuleType & type);
 
+lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const LaneChangeModuleType & type);
+
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -679,21 +679,13 @@ bool NormalLaneChange::getLaneChangePaths(
   const auto dist_to_end_of_current_lanes =
     utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
 
-  const auto arc_position_from_target =
-    lanelet::utils::getArcCoordinates(target_lanes, getEgoPose());
-
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
 
-  const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds(
-    route_handler, current_lanes, target_lanes, arc_position_from_target.distance);
-
-  const auto target_neighbor_lanelets =
-    utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type_);
+  const auto sorted_lane_ids =
+    utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes);
 
-  const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength(
-    target_neighbor_lanelets, 0, std::numeric_limits<double>::max());
   const auto target_neighbor_preferred_lane_poly_2d =
-    lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon();
+    utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
 

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -203,6 +203,18 @@ lanelet::ConstLanelets getTargetNeighborLanes(
   return neighbor_lanes;
 }
 
+lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const LaneChangeModuleType & type)
+{
+  const auto target_neighbor_lanelets =
+    utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type);
+  const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength(
+    target_neighbor_lanelets, 0, std::numeric_limits<double>::max());
+
+  return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon();
+}
+
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes)
@@ -640,9 +652,12 @@ std::string getStrDirection(const std::string & name, const Direction direction)
 }
 
 std::vector<std::vector<int64_t>> getSortedLaneIds(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes, const double rough_shift_length)
+  const RouteHandler & route_handler, const Pose & current_pose,
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes)
 {
+  const auto rough_shift_length =
+    lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance;
+
   std::vector<std::vector<int64_t>> sorted_lane_ids{};
   sorted_lane_ids.reserve(target_lanes.size());
   const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) {