From aecf0447526dca2f87d1d4d52f23516fae9c0c08 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 20 Nov 2023 10:28:00 +0900 Subject: [PATCH] fix(intersection): fix infinite loop in tsort (#5332) (#1022) Signed-off-by: Mamoru Sobue Co-authored-by: Mamoru Sobue --- .../src/util.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 67e5d552771a0..f605041e22658 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/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{}; auto & branch = branches[(ind2id[src])]; lanelet::Id node_iter = ind2id[src]; + std::set 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)]; } }