refactor(behavior_path_planner): clean the code of common drivable ar…

…ea generation (autowarefoundation#3246)

* refactor(behavior_path_planner): clean the code of common drivable area generation

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/behavior_path_planner/src/util/lane_change/util.cpp

@@ -910,54 +910,54 @@ std::vector<DrivableLanes> generateDrivableLanes(
       return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end();
     };
 
-  const auto checkMiddle = [&](const auto & lane) {
+  const auto checkMiddle = [&](const auto & lane) -> std::optional<DrivableLanes> {
     for (const auto & drivable_lane : original_drivable_lanes) {
       if (has_same_lane(drivable_lane.middle_lanes, lane)) {
-        return std::make_pair(true, drivable_lane);
+        return drivable_lane;
       }
     }
-    return std::make_pair(false, DrivableLanes());
+    return std::nullopt;
   };
 
-  const auto checkLeft = [&](const auto & lane) {
+  const auto checkLeft = [&](const auto & lane) -> std::optional<DrivableLanes> {
     for (const auto & drivable_lane : original_drivable_lanes) {
       if (drivable_lane.left_lane.id() == lane.id()) {
-        return std::make_pair(true, drivable_lane);
+        return drivable_lane;
       }
     }
-    return std::make_pair(false, DrivableLanes());
+    return std::nullopt;
   };
 
-  const auto checkRight = [&](const auto & lane) {
+  const auto checkRight = [&](const auto & lane) -> std::optional<DrivableLanes> {
     for (const auto & drivable_lane : original_drivable_lanes) {
       if (drivable_lane.right_lane.id() == lane.id()) {
-        return std::make_pair(true, drivable_lane);
+        return drivable_lane;
       }
     }
-    return std::make_pair(false, DrivableLanes());
+    return std::nullopt;
   };
 
   size_t current_lc_idx = 0;
   std::vector<DrivableLanes> drivable_lanes(current_lanes.size());
   for (size_t i = 0; i < current_lanes.size(); ++i) {
     const auto & current_lane = current_lanes.at(i);
 
-    const auto [is_middle, drivable_lane_1] = checkMiddle(current_lane);
-    if (is_middle) {
-      drivable_lanes.at(i) = drivable_lane_1;
+    const auto middle_drivable_lane = checkMiddle(current_lane);
+    if (middle_drivable_lane) {
+      drivable_lanes.at(i) = *middle_drivable_lane;
     }
 
-    const auto [is_left, drivable_lane_2] = checkLeft(current_lane);
-    if (is_left) {
-      drivable_lanes.at(i) = drivable_lane_2;
+    const auto left_drivable_lane = checkLeft(current_lane);
+    if (left_drivable_lane) {
+      drivable_lanes.at(i) = *left_drivable_lane;
     }
 
-    const auto [is_right, drivable_lane_3] = checkRight(current_lane);
-    if (is_right) {
-      drivable_lanes.at(i) = drivable_lane_3;
+    const auto right_drivable_lane = checkRight(current_lane);
+    if (right_drivable_lane) {
+      drivable_lanes.at(i) = *right_drivable_lane;
     }
 
-    if (!is_middle && !is_left && !is_right) {
+    if (!middle_drivable_lane && !left_drivable_lane && !right_drivable_lane) {
       drivable_lanes.at(i).left_lane = current_lane;
       drivable_lanes.at(i).right_lane = current_lane;
     }
@@ -971,15 +971,15 @@ std::vector<DrivableLanes> generateDrivableLanes(
     for (size_t lc_idx = current_lc_idx; lc_idx < lane_change_lanes.size(); ++lc_idx) {
       const auto & lc_lane = lane_change_lanes.at(lc_idx);
       if (left_lane && lc_lane.id() == left_lane->id()) {
-        if (is_left) {
+        if (left_drivable_lane) {
           drivable_lanes.at(i).left_lane = lc_lane;
         }
         current_lc_idx = lc_idx;
         break;
       }
 
       if (right_lane && lc_lane.id() == right_lane->id()) {
-        if (is_right) {
+        if (right_drivable_lane) {
           drivable_lanes.at(i).right_lane = lc_lane;
         }
         current_lc_idx = lc_idx;
@@ -992,22 +992,22 @@ std::vector<DrivableLanes> generateDrivableLanes(
     const auto & lc_lane = lane_change_lanes.at(i);
     DrivableLanes drivable_lane;
 
-    const auto [is_middle, drivable_lane_1] = checkMiddle(lc_lane);
-    if (is_middle) {
-      drivable_lane = drivable_lane_1;
+    const auto middle_drivable_lane = checkMiddle(lc_lane);
+    if (middle_drivable_lane) {
+      drivable_lane = *middle_drivable_lane;
     }
 
-    const auto [is_left, drivable_lane_2] = checkLeft(lc_lane);
-    if (is_left) {
-      drivable_lane = drivable_lane_2;
+    const auto left_drivable_lane = checkLeft(lc_lane);
+    if (left_drivable_lane) {
+      drivable_lane = *left_drivable_lane;
     }
 
-    const auto [is_right, drivable_lane_3] = checkRight(lc_lane);
-    if (is_right) {
-      drivable_lane = drivable_lane_3;
+    const auto right_drivable_lane = checkRight(lc_lane);
+    if (right_drivable_lane) {
+      drivable_lane = *right_drivable_lane;
     }
 
-    if (!is_middle && !is_left && !is_right) {
+    if (!middle_drivable_lane && !left_drivable_lane && !right_drivable_lane) {
       drivable_lane.left_lane = lc_lane;
       drivable_lane.right_lane = lc_lane;
     }