refactor(avoidance_module): change implementation to lambda

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -241,38 +241,23 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
       lanelet::BasicPoint3d overhang_basic_pose(
         object_data.overhang_pose.position.x, object_data.overhang_pose.position.y,
         object_data.overhang_pose.position.z);
+      const bool get_left =
+        isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction;
+      const bool get_right =
+        !isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction;
+
+      const auto target_lines = rh->getFurthestLinestring(
+        overhang_lanelet, get_right, get_left,
+        parameters_.enable_avoidance_over_opposite_direction);
+
       if (isOnRight(object_data)) {
-        const auto & target_left_line = [this, &rh, &overhang_lanelet]() {
-          if (
-            parameters_.enable_avoidance_over_same_direction &&
-            parameters_.enable_avoidance_over_opposite_direction) {
-            return rh->getLeftMostLinestring(overhang_lanelet);
-          } else if (
-            parameters_.enable_avoidance_over_same_direction &&
-            !parameters_.enable_avoidance_over_opposite_direction) {
-            return rh->getLeftMostSameDirectionLinestring(overhang_lanelet);
-          }
-          return overhang_lanelet.leftBound();
-        }();
         object_data.to_road_shoulder_distance =
-          distance2d(to2D(overhang_basic_pose), to2D(target_left_line.basicLineString()));
-        debug_linestring.push_back(target_left_line);
+          distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString()));
+        debug_linestring.push_back(target_lines.back());
       } else {
-        const auto & target_right_line = [this, &rh, &overhang_lanelet]() {
-          if (
-            parameters_.enable_avoidance_over_same_direction &&
-            parameters_.enable_avoidance_over_opposite_direction) {
-            return rh->getRightMostLinestring(overhang_lanelet);
-          } else if (
-            parameters_.enable_avoidance_over_same_direction &&
-            !parameters_.enable_avoidance_over_opposite_direction) {
-            return rh->getRightMostSameDirectionLinestring(overhang_lanelet);
-          }
-          return overhang_lanelet.rightBound();
-        }();
         object_data.to_road_shoulder_distance =
-          distance2d(to2D(overhang_basic_pose), to2D(target_right_line.basicLineString()));
-        debug_linestring.push_back(target_right_line);
+          distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString()));
+        debug_linestring.push_back(target_lines.front());
       }
     }
 
@@ -1675,35 +1660,20 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
   {
     // 0. Extend to right/left of objects
     for (const auto & obstacle : avoidance_data_.objects) {
+      lanelet::ConstLanelets search_lanelets;
       auto object_lanelet = obstacle.overhang_lanelet;
+      constexpr bool get_right = true;
+      constexpr bool get_left = true;
+      const bool include_opposite = parameters_.enable_avoidance_over_opposite_direction;
       if (isOnRight(obstacle)) {
-        auto lanelet_at_left = route_handler->getLeftLanelet(object_lanelet);
-        while (lanelet_at_left) {
-          extended_lanelets.push_back(lanelet_at_left.get());
-          lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
-        }
-        if (lanelet_at_left) {
-          auto lanelet_at_right =
-            planner_data_->route_handler->getRightLanelet(lanelet_at_left.get());
-          while (lanelet_at_right) {
-            extended_lanelets.push_back(lanelet_at_right.get());
-            lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
-          }
-        }
+        search_lanelets = route_handler->getAllSharedLineStringLanelets(
+          object_lanelet, !get_right, get_left, include_opposite);
       } else {
-        auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet);
-        while (lanelet_at_right) {
-          extended_lanelets.push_back(lanelet_at_right.get());
-          lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
-        }
-        if (lanelet_at_right) {
-          auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get());
-          while (lanelet_at_left) {
-            extended_lanelets.push_back(lanelet_at_left.get());
-            lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
-          }
-        }
+        search_lanelets = route_handler->getAllSharedLineStringLanelets(
+          object_lanelet, get_right, !get_left, include_opposite);
       }
+      extended_lanelets.insert(
+        extended_lanelets.end(), search_lanelets.begin(), search_lanelets.end());
     }
   }
 

planning/route_handler/include/route_handler/route_handler.hpp

@@ -138,6 +138,36 @@ class RouteHandler
    */
   lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const;
 
+  /**
+   * @brief Searches and return all lanelet on the left that shares same linestring
+   * @param the lanelet of interest
+   * @param (optional) flag to include the lane with opposite direction
+   * @return vector of lanelet that is connected via share linestring
+   */
+  lanelet::ConstLanelets getAllLeftSharedLinestringLanelets(
+    const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept;
+
+  /**
+   * @brief Searches and return all lanelet on the right that shares same linestring
+   * @param the lanelet of interest
+   * @param (optional) flag to include the lane with opposite direction
+   * @return vector of lanelet that is connected via share linestring
+   */
+  lanelet::ConstLanelets getAllRightSharedLinestringLanelets(
+    const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept;
+
+  /**
+   * @brief Searches and return all lanelet (left and right) that shares same linestring
+   * @param the lanelet of interest
+   * @param (optional) flag to search only right side
+   * @param (optional) flag to search only left side
+   * @param (optional) flag to include the lane with opposite direction
+   * @return vector of lanelet that is connected via share linestring
+   */
+  lanelet::ConstLanelets getAllSharedLineStringLanelets(
+    const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true,
+    bool is_opposite = true) const noexcept;
+
   /**
    * @brief Searches the furthest linestring to the right side of the lanelet
    * Only lanelet with same direction is considered
@@ -173,6 +203,19 @@ class RouteHandler
    */
   lanelet::ConstLineString3d getLeftMostLinestring(
     const lanelet::ConstLanelet & lanelet) const noexcept;
+
+  /**
+   * @brief Return furthest linestring on both side of the lanelet
+   * @param the lanelet of interest
+   * @param (optional) search furthest right side
+   * @param (optional) search furthest left side
+   * @param (optional) include opposite lane as well
+   * @return right and left linestrings
+   */
+  lanelet::ConstLineStrings3d getFurthestLinestring(
+    const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true,
+    bool is_opposite = true) const noexcept;
+
   int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const;
   bool getClosestLaneletWithinRoute(
     const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const;

planning/route_handler/src/route_handler.cpp

@@ -911,6 +911,73 @@ lanelet::Lanelets RouteHandler::getRightOppositeLanelets(
   return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert());
 }
 
+lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets(
+  const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept
+{
+  lanelet::ConstLanelets linestring_shared;
+  auto lanelet_at_left = getLeftLanelet(lane);
+  auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane);
+  while (lanelet_at_left) {
+    linestring_shared.push_back(lanelet_at_left.get());
+    lanelet_at_left = getLeftLanelet(lanelet_at_left.get());
+    lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get());
+  }
+
+  if (!lanelet_at_left_opposite.empty() && include_opposite) {
+    linestring_shared.push_back(lanelet_at_left_opposite.front());
+    auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front());
+    while (lanelet_at_right) {
+      linestring_shared.push_back(lanelet_at_right.get());
+      lanelet_at_right = getRightLanelet(lanelet_at_right.get());
+    }
+  }
+  return linestring_shared;
+}
+
+lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets(
+  const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept
+{
+  lanelet::ConstLanelets linestring_shared;
+  auto lanelet_at_right = getRightLanelet(lane);
+  auto lanelet_at_right_opposite = getRightOppositeLanelets(lane);
+  while (lanelet_at_right) {
+    linestring_shared.push_back(lanelet_at_right.get());
+    lanelet_at_right = getRightLanelet(lanelet_at_right.get());
+    lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get());
+  }
+
+  if (!lanelet_at_right_opposite.empty() && include_opposite) {
+    linestring_shared.push_back(lanelet_at_right_opposite.front());
+    auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front());
+    while (lanelet_at_left) {
+      linestring_shared.push_back(lanelet_at_left.get());
+      lanelet_at_left = getLeftLanelet(lanelet_at_left.get());
+    }
+  }
+  return linestring_shared;
+}
+
+lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets(
+  const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left,
+  bool is_opposite) const noexcept
+{
+  lanelet::ConstLanelets shared{current_lane};
+
+  if (is_right) {
+    const lanelet::ConstLanelets all_right_lanelets =
+      getAllRightSharedLinestringLanelets(current_lane, is_opposite);
+    shared.insert(shared.end(), all_right_lanelets.begin(), all_right_lanelets.end());
+  }
+
+  if (is_left) {
+    const lanelet::ConstLanelets all_left_lanelets =
+      getAllLeftSharedLinestringLanelets(current_lane, is_opposite);
+    shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end());
+  }
+
+  return shared;
+}
+
 lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const
 {
   return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert());
@@ -972,6 +1039,31 @@ lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring(
   return {};
 }
 
+lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring(
+  const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left,
+  bool is_opposite) const noexcept
+{
+  lanelet::ConstLineStrings3d linestrings;
+  linestrings.reserve(2);
+
+  if (is_right && is_opposite) {
+    linestrings.emplace_back(getRightMostLinestring(lanelet));
+  } else if (is_right && !is_opposite) {
+    linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet));
+  } else {
+    linestrings.emplace_back(lanelet.rightBound());
+  }
+
+  if (is_left && is_opposite) {
+    linestrings.emplace_back(getLeftMostLinestring(lanelet));
+  } else if (is_left && !is_opposite) {
+    linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet));
+  } else {
+    linestrings.emplace_back(lanelet.leftBound());
+  }
+  return linestrings;
+}
+
 bool RouteHandler::getLaneChangeTarget(
   const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const
 {