feat(route_handler): add invert flag (autowarefoundation#2344)

* feat(route_handler): add invert flag

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

planning/route_handler/include/route_handler/route_handler.hpp

@@ -145,31 +145,36 @@ class RouteHandler
    * @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
+   * @param (optional) flag to invert the opposite lanelet
    * @return vector of lanelet that is connected via share linestring
    */
   lanelet::ConstLanelets getAllLeftSharedLinestringLanelets(
-    const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept;
+    const lanelet::ConstLanelet & lane, const bool & include_opposite,
+    const bool & invert_opposite = false) 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
+   * @param (optional) flag to invert the opposite lanelet
    * @return vector of lanelet that is connected via share linestring
    */
   lanelet::ConstLanelets getAllRightSharedLinestringLanelets(
-    const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept;
+    const lanelet::ConstLanelet & lane, const bool & include_opposite,
+    const bool & invert_opposite = false) 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
+   * @param (optional) flag to invert the opposite lanelet
    * @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;
+    bool is_opposite = true, const bool & invert_opposite = false) const noexcept;
 
   /**
    * @brief Searches the furthest linestring to the right side of the lanelet

planning/route_handler/src/route_handler.cpp

@@ -892,7 +892,8 @@ lanelet::Lanelets RouteHandler::getRightOppositeLanelets(
 }
 
 lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets(
-  const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept
+  const lanelet::ConstLanelet & lane, const bool & include_opposite,
+  const bool & invert_opposite) const noexcept
 {
   lanelet::ConstLanelets linestring_shared;
   auto lanelet_at_left = getLeftLanelet(lane);
@@ -907,18 +908,27 @@ lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets(
   }
 
   if (!lanelet_at_left_opposite.empty() && include_opposite) {
-    linestring_shared.push_back(lanelet_at_left_opposite.front());
+    if (invert_opposite) {
+      linestring_shared.push_back(lanelet_at_left_opposite.front().invert());
+    } else {
+      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());
+      if (invert_opposite) {
+        linestring_shared.push_back(lanelet_at_right.get().invert());
+      } else {
+        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
+  const lanelet::ConstLanelet & lane, const bool & include_opposite,
+  const bool & invert_opposite) const noexcept
 {
   lanelet::ConstLanelets linestring_shared;
   auto lanelet_at_right = getRightLanelet(lane);
@@ -933,31 +943,39 @@ lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets(
   }
 
   if (!lanelet_at_right_opposite.empty() && include_opposite) {
-    linestring_shared.push_back(lanelet_at_right_opposite.front());
+    if (invert_opposite) {
+      linestring_shared.push_back(lanelet_at_right_opposite.front().invert());
+    } else {
+      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());
+      if (invert_opposite) {
+        linestring_shared.push_back(lanelet_at_left.get().invert());
+      } else {
+        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
+  const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left, bool is_opposite,
+  const bool & invert_opposite) const noexcept
 {
   lanelet::ConstLanelets shared{current_lane};
 
   if (is_right) {
     const lanelet::ConstLanelets all_right_lanelets =
-      getAllRightSharedLinestringLanelets(current_lane, is_opposite);
+      getAllRightSharedLinestringLanelets(current_lane, is_opposite, invert_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);
+      getAllLeftSharedLinestringLanelets(current_lane, is_opposite, invert_opposite);
     shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end());
   }