From 3c6d5aa61049ac2cb449f6bbbd79ac2ba5c7e256 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Sun, 13 Aug 2023 13:21:32 +0900
Subject: [PATCH] feat(route_handler): support shoulder lane in
 getLeft(Right)Lanelet (#4604)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 planning/route_handler/src/route_handler.cpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index 4e009836b53d4..26d226a6746e2 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -942,6 +942,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) {
@@ -999,6 +1009,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) {