refactor(avoidance_module): change implementation to lambda (tier4#486)

* refactor(avoidance_module): change implementation to lambda

In the generateExtendedDrivableArea function, the implementation to extend
drivable areas with reference to object is refactor to lambda.

This will allow the function to be used for some future planned updates.

The refactor doesn't affect the behavior of the avoidance_module.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor: add default flag to extend_to_opposite side

By default, the parameter is true, therefore it doesn't affect the original
behavior.

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* ci(pre-commit): autofix

* fix: rework function for better extension

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor(route handler, avoidance_module): move the implementation

The implementation is made into function and moved into route handler

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Refactor: convert lambdas to function

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* chore: add comments for comments for documentation in the header file

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Refactor: local variable rename

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* refactor: make a wrapper function to get furthest linestrings

Also modified some avoidance_module to include the refactored functions

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

@@ -255,38 +255,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());
       }
     }
 
@@ -1732,35 +1717,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
 {