feat(route_handler): support shoulder lane in getLeft(Right)Lanelet (a…

…utowarefoundation#4604) Signed-off-by: kosuke55 <[email protected]>
tier4 · Aug 22, 2023 · 8bd25f3 · 8bd25f3
1 parent 51480eb
commit 8bd25f3
Showing 1 changed file with 20 additions and 0 deletions.
diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
@@ -931,6 +931,16 @@ bool RouteHandler::isBijectiveConnection(
 boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
   const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
 {
+  // right road lanelet of shoulder lanelet
+  if (isShoulderLanelet(lanelet)) {
+    for (const auto & road_lanelet : road_lanelets_) {
+      if (lanelet::geometry::rightOf(road_lanelet, lanelet)) {
+        return road_lanelet;
+      }
+    }
+    return boost::none;
+  }
+
   // routable lane
   const auto & right_lane = routing_graph_ptr_->right(lanelet);
   if (right_lane) {
@@ -988,6 +998,16 @@ bool RouteHandler::getLeftLaneletWithinRoute(
 boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
   const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const
 {
+  // left road lanelet of shoulder lanelet
+  if (isShoulderLanelet(lanelet)) {
+    for (const auto & road_lanelet : road_lanelets_) {
+      if (lanelet::geometry::leftOf(road_lanelet, lanelet)) {
+        return road_lanelet;
+      }
+    }
+    return boost::none;
+  }
+
   // routable lane
   const auto & left_lane = routing_graph_ptr_->left(lanelet);
   if (left_lane) {