feat(route_handler): better avoidance drivable areas extension in beh…

…avior path planning (#285) * rebase to develop [Zulfaqar Azmi] route handler function for better drivable area extension this is a part of .iv PR (tier4/autoware.iv#2383) port Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * route handler function for checking adjacent lane availability this is a part of .iv PR (tier4/autoware.iv#2383) port Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Jan 24, 2022 · 10bd1c0 · 10bd1c0
1 parent 7ecbd72
commit 10bd1c0
Show file tree

Hide file tree

Showing 2 changed files with 181 additions and 2 deletions.
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -90,7 +90,83 @@ class RouteHandler
   std::vector<lanelet::ConstLanelet> getLanesAfterGoal(const double vehicle_length) const;
 
   // for lanelet
+  bool getPreviousLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
   bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const;
+
+  /**
+   * @brief Check if same-direction lane is available at the right side of the lanelet
+   * Searches for any lanes regardless of whether it is lane-changeable or not.
+   * Required the linestring to be shared(same line ID) between the lanelets.
+   * @param the lanelet of interest
+   * @return vector of lanelet having same direction if true
+   */
+  boost::optional<lanelet::ConstLanelet> getRightLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+
+  /**
+   * @brief Check if same-direction lane is available at the left side of the lanelet
+   * Searches for any lanes regardless of whether it is lane-changeable or not.
+   * Required the linestring to be shared(same line ID) between the lanelets.
+   * @param the lanelet of interest
+   * @return vector of lanelet having same direction if true
+   */
+  boost::optional<lanelet::ConstLanelet> getLeftLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const;
+
+  /**
+   * @brief Check if opposite-direction lane is available at the right side of the lanelet
+   * Required the linestring to be shared(same line ID) between the lanelets.
+   * @param the lanelet of interest
+   * @return vector of lanelet with opposite direction if true
+   */
+  lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet) const;
+
+  /**
+   * @brief Check if opposite-direction lane is available at the left side of the lanelet
+   * Required the linestring to be shared between(same line ID) the lanelets.
+   * @param the lanelet of interest
+   * @return vector of lanelet with opposite direction if true
+   */
+  lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const;
+
+  /**
+   * @brief Searches the furthest linestring to the right side of the lanelet
+   * Only lanelet with same direction is considered
+   * @param the lanelet of interest
+   * @return right most linestring of the lane with same direction
+   */
+  lanelet::ConstLineString3d getRightMostSameDirectionLinestring(
+    const lanelet::ConstLanelet & lanelet) const noexcept;
+
+  /**
+   * @brief Searches the furthest linestring to the right side of the lanelet
+   * Used to search for road shoulders. Lane direction is ignored
+   * @param the lanelet of interest
+   * @return right most linestring
+   */
+  lanelet::ConstLineString3d getRightMostLinestring(
+    const lanelet::ConstLanelet & lanelet) const noexcept;
+
+  /**
+   * @brief Searches the furthest linestring to the left side of the lanelet
+   * Only lanelet with same direction is considered
+   * @param the lanelet of interest
+   * @return left most linestring of the lane with same direction
+   */
+  lanelet::ConstLineString3d getLeftMostSameDirectionLinestring(
+    const lanelet::ConstLanelet & lanelet) const noexcept;
+
+  /**
+   * @brief Searches the furthest linestring to the left side of the lanelet
+   * Used to search for road shoulders. Lane direction is ignored
+   * @param the lanelet of interest
+   * @return left most linestring
+   */
+  lanelet::ConstLineString3d getLeftMostLinestring(
+    const lanelet::ConstLanelet & lanelet) const noexcept;
   int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const;
   bool getClosestLaneletWithinRoute(
     const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const;
@@ -163,8 +239,6 @@ class RouteHandler
   bool isBijectiveConnection(
     const lanelet::ConstLanelets & lanelet_section1,
     const lanelet::ConstLanelets & lanelet_section2) const;
-  bool getPreviousLaneletWithinRoute(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
   bool getNextLaneletWithinRoute(
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
   bool getPreviousLaneletWithinRouteExceptGoal(

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
@@ -739,6 +739,11 @@ bool RouteHandler::getNextLaneletWithinRoute(
   return false;
 }
 
+lanelet::ConstLanelets RouteHandler::getNextLanelets(const lanelet::ConstLanelet & lanelet) const
+{
+  return routing_graph_ptr_->following(lanelet);
+}
+
 bool RouteHandler::getPreviousLaneletWithinRoute(
   const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const
 {
@@ -755,6 +760,11 @@ bool RouteHandler::getPreviousLaneletWithinRoute(
   return false;
 }
 
+lanelet::ConstLanelets RouteHandler::getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const
+{
+  return lanelet::utils::findUsagesInLanelets(*lanelet_map_ptr_, point);
+}
+
 bool RouteHandler::getRightLaneletWithinRoute(
   const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const
 {
@@ -831,6 +841,20 @@ bool RouteHandler::isBijectiveConnection(
   return true;
 }
 
+boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  // routable lane
+  const auto & right_lane = routing_graph_ptr_->right(lanelet);
+  if (right_lane) {
+    return right_lane;
+  }
+
+  // non-routable lane (e.g. lane change infeasible)
+  const auto & adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet);
+  return adjacent_right_lane;
+}
+
 bool RouteHandler::getLeftLaneletWithinRoute(
   const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const
 {
@@ -843,6 +867,87 @@ bool RouteHandler::getLeftLaneletWithinRoute(
   }
 }
 
+boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  // routable lane
+  const auto & left_lane = routing_graph_ptr_->left(lanelet);
+  if (left_lane) {
+    return left_lane;
+  }
+
+  // non-routable lane (e.g. lane change infeasible)
+  const auto & adjacent_left_lane = routing_graph_ptr_->adjacentLeft(lanelet);
+  return adjacent_left_lane;
+}
+
+lanelet::Lanelets RouteHandler::getRightOppositeLanelets(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert());
+}
+
+lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const
+{
+  return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert());
+}
+
+lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring(
+  const lanelet::ConstLanelet & lanelet) const noexcept
+{
+  // recursively compute the width of the lanes
+  const auto & same = getRightLanelet(lanelet);
+
+  if (same) {
+    return getRightMostSameDirectionLinestring(same.get());
+  }
+  return lanelet.rightBound();
+}
+
+lanelet::ConstLineString3d RouteHandler::getRightMostLinestring(
+  const lanelet::ConstLanelet & lanelet) const noexcept
+{
+  const auto & same = getRightLanelet(lanelet);
+  const auto & opposite = getRightOppositeLanelets(lanelet);
+  if (!same && opposite.empty()) {
+    return lanelet.rightBound();
+  } else if (same) {
+    return getRightMostLinestring(same.get());
+  } else if (!opposite.empty()) {
+    return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front()));
+  }
+  return {};
+}
+
+lanelet::ConstLineString3d RouteHandler::getLeftMostSameDirectionLinestring(
+  const lanelet::ConstLanelet & lanelet) const noexcept
+{
+  // recursively compute the width of the lanes
+  const auto & same = getLeftLanelet(lanelet);
+
+  if (same) {
+    return getLeftMostSameDirectionLinestring(same.get());
+  }
+  return lanelet.leftBound();
+}
+
+lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring(
+  const lanelet::ConstLanelet & lanelet) const noexcept
+{
+  // recursively compute the width of the lanes
+  const auto & same = getLeftLanelet(lanelet);
+  const auto & opposite = getLeftOppositeLanelets(lanelet);
+
+  if (!same && opposite.empty()) {
+    return lanelet.leftBound();
+  } else if (same) {
+    return getLeftMostLinestring(same.get());
+  } else if (!opposite.empty()) {
+    return getRightMostLinestring(lanelet::ConstLanelet(opposite.front()));
+  }
+  return {};
+}
+
 bool RouteHandler::getLaneChangeTarget(
   const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const
 {