fix(route_handle): query shoulder lane in getLaneletSequence() if onl…

…y_route_lanes is false and the arg is shoulder

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/route_handler/src/route_handler.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -643,12 +643,22 @@
     return lanelet_sequence;
   }
 
-  lanelet::ConstLanelets lanelet_sequence_forward =
-    getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes);
+  const lanelet::ConstLanelets lanelet_sequence_forward = std::invoke([&]() {
+    if (only_route_lanes) {
+      return getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes);
+    } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) {
+      return getShoulderLaneletSequenceAfter(lanelet, forward_distance);
+    }
+    return lanelet::ConstLanelets{};
+  });
   const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() {
     const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose);
     if (arc_coordinate.length < backward_distance) {
-      return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes);
+      if (only_route_lanes) {
+        return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes);
+      } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) {
+        return getShoulderLaneletSequenceUpTo(lanelet, backward_distance);
+      }
     }
     return lanelet::ConstLanelets{};
   });