ref(traffic_simulator): improve hdmaputils::countLaneChanges

tier4 · Dec 10, 2024 · 768b84e · 768b84e
1 parent bd67439
commit 768b84e
Showing 1 changed file with 12 additions and 15 deletions.
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -86,7 +86,6 @@ HdMapUtils::HdMapUtils(
 
 // If route is not specified, the lanelet_id with the lowest array index is used as a candidate for
 // canonicalize destination.
-
 auto HdMapUtils::countLaneChanges(
   const traffic_simulator_msgs::msg::LaneletPose & from,
   const traffic_simulator_msgs::msg::LaneletPose & to,
@@ -102,20 +101,18 @@ auto HdMapUtils::countLaneChanges(
     for (std::size_t i = 1; i < route.size(); ++i) {
       const auto & previous = route[i - 1];
       const auto & current = route[i];
-
-      if (auto followings =
-            lanelet_map::nextLaneletIds(previous, routing_configuration.routing_graph_type);
-          std::find(followings.begin(), followings.end(), current) == followings.end()) {
-        if (auto lefts = pose::leftLaneletIds(
-              previous, routing_configuration.routing_graph_type, include_opposite_direction);
-            std::find(lefts.begin(), lefts.end(), current) != lefts.end()) {
-          lane_changes.first++;
-        } else if (auto rights = pose::rightLaneletIds(
-                     previous, routing_configuration.routing_graph_type,
-                     include_opposite_direction);
-                   std::find(rights.begin(), rights.end(), current) != rights.end()) {
-          lane_changes.second++;
-        }
+      if (const auto followings = lanelet_map::nextLaneletIds(
+            previous, routing_configuration.routing_graph_type, include_opposite_direction);
+          std::find(followings.begin(), followings.end(), current) != followings.end()) {
+        continue;
+      } else if (const auto lefts = pose::leftLaneletIds(
+                   previous, routing_configuration.routing_graph_type, include_opposite_direction);
+                 std::find(lefts.begin(), lefts.end(), current) != lefts.end()) {
+        lane_changes.first++;
+      } else if (const auto rights = pose::rightLaneletIds(
+                   previous, routing_configuration.routing_graph_type, include_opposite_direction);
+                 std::find(rights.begin(), rights.end(), current) != rights.end()) {
+        lane_changes.second++;
       }
     }
     return lane_changes;