fix(intersection): fix infinite loop in tsort (autowarefoundation#5332)…

… (#1022)

Signed-off-by: Mamoru Sobue <[email protected]>
Co-authored-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -602,6 +602,8 @@ mergeLaneletsByTopologicalSort(
     id2lanelet[id] = lanelet;
     ind++;
   }
+  // NOTE: this function aims to traverse the detection lanelet backward from ego side to farthest
+  // side, so if lane B follows lane A on the routing_graph, adj[B][A] = true
   for (const auto & lanelet : lanelets) {
     const auto & followings = routing_graph_ptr->following(lanelet);
     const int dst = lanelet.id();
@@ -625,18 +627,25 @@ mergeLaneletsByTopologicalSort(
     if (!has_no_previous(src)) {
       continue;
     }
+    // So `src` has no previous lanelets
     branches[(ind2id[src])] = std::vector<lanelet::Id>{};
     auto & branch = branches[(ind2id[src])];
     lanelet::Id node_iter = ind2id[src];
+    std::set<lanelet::Id> visited_ids;
     while (true) {
       const auto & destinations = adjacency[(id2ind[node_iter])];
-      // NOTE: assuming detection lanelets have only one previous lanelet
+      // NOTE: assuming detection lanelets have only one "previous"(on the routing_graph) lanelet
       const auto next = std::find(destinations.begin(), destinations.end(), true);
       if (next == destinations.end()) {
         branch.push_back(node_iter);
         break;
       }
+      if (visited_ids.find(node_iter) != visited_ids.end()) {
+        // loop detected
+        break;
+      }
       branch.push_back(node_iter);
+      visited_ids.insert(node_iter);
       node_iter = ind2id[std::distance(destinations.begin(), next)];
     }
   }