refactor: add routing graph argument to HdMapUtils::getLaneChangeable…

…LaneletId
tier4 · Nov 25, 2024 · 4dedb32 · 4dedb32
1 parent 7d8f308
commit 4dedb32
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 14 deletions.
diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
@@ -168,8 +168,9 @@ class HdMapUtils
     -> std::optional<std::pair<math::geometry::HermiteCurve, double>>;
 
   auto getLaneChangeableLaneletId(
-    const lanelet::Id, const traffic_simulator::lane_change::Direction) const
-    -> std::optional<lanelet::Id>;
+    const lanelet::Id, const traffic_simulator::lane_change::Direction,
+    const traffic_simulator::RoutingGraphType type =
+      traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional<lanelet::Id>;
 
   auto getLaneChangeableLaneletId(
     const lanelet::Id, const traffic_simulator::lane_change::Direction,

diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -762,8 +762,8 @@ auto HdMapUtils::getLaneChangeableLaneletId(
 }
 
 auto HdMapUtils::getLaneChangeableLaneletId(
-  const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction) const
-  -> std::optional<lanelet::Id>
+  const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction,
+  const traffic_simulator::RoutingGraphType type) const -> std::optional<lanelet::Id>
 {
   const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id);
   std::optional<lanelet::Id> target = std::nullopt;
@@ -772,19 +772,13 @@ auto HdMapUtils::getLaneChangeableLaneletId(
       target = lanelet.id();
       break;
     case traffic_simulator::lane_change::Direction::LEFT:
-      if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)
-            ->left(lanelet)) {
-        target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)
-                   ->left(lanelet)
-                   ->id();
+      if (routing_graphs_->routing_graph(type)->left(lanelet)) {
+        target = routing_graphs_->routing_graph(type)->left(lanelet)->id();
       }
       break;
     case traffic_simulator::lane_change::Direction::RIGHT:
-      if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)
-            ->right(lanelet)) {
-        target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)
-                   ->right(lanelet)
-                   ->id();
+      if (routing_graphs_->routing_graph(type)->right(lanelet)) {
+        target = routing_graphs_->routing_graph(type)->right(lanelet)->id();
       }
       break;
   }