fix(intersection): additional fix for 8520 (autowarefoundation#8561)

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

Author: soblin

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

@@ -934,6 +934,14 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
     detection_lanelets.push_back(detection_lanelet);
   }
 
+  std::vector<lanelet::ConstLineString3d> detection_divisions;
+  if (detection_lanelets.empty()) {
+    // NOTE(soblin): due to the above filtering detection_lanelets may be empty or do not contain
+    // conflicting_detection_lanelets
+    // OK to return empty detction_divsions
+    return detection_divisions;
+  }
+
   // (1) tsort detection_lanelets
   const auto [merged_detection_lanelets, originals] = util::mergeLaneletsByTopologicalSort(
     detection_lanelets, conflicting_detection_lanelets, routing_graph_ptr);
@@ -952,7 +960,6 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
   }
 
   // (3) discretize each merged lanelet
-  std::vector<lanelet::ConstLineString3d> detection_divisions;
   for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) {
     const double length = bg::length(merged_lanelet.centerline());
     const double width = area / length;

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp

@@ -32,10 +32,13 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
+#include <map>
+#include <memory>
+#include <queue>
 #include <set>
 #include <string>
 #include <unordered_map>
-#include <unordered_set>
 #include <vector>
 
 namespace autoware::behavior_velocity_planner::util
@@ -212,42 +215,35 @@ std::optional<size_t> getFirstPointInsidePolygon(
   }
   return std::nullopt;
 }
-std::vector<std::vector<size_t>> retrievePathsBackward(
-  const std::vector<std::vector<bool>> & adjacency, size_t start_node)
+
+void retrievePathsBackward(
+  const std::vector<std::vector<bool>> & adjacency, const size_t src_ind,
+  const std::vector<size_t> & visited_inds, std::vector<std::vector<size_t>> & paths)
 {
-  std::vector<std::vector<size_t>> paths;
-  std::vector<size_t> current_path;
-  std::unordered_set<size_t> visited;
-
-  std::function<void(size_t)> retrieve_paths_backward_impl = [&](size_t src_ind) {
-    current_path.push_back(src_ind);
-    visited.insert(src_ind);
-
-    bool is_terminal = true;
-    const auto & nexts = adjacency[src_ind];
-
-    for (size_t next = 0; next < nexts.size(); ++next) {
-      if (nexts[next]) {
-        is_terminal = false;
-        if (visited.find(next) == visited.end()) {
-          retrieve_paths_backward_impl(next);
-        } else {
-          // Loop detected
-          paths.push_back(current_path);
-        }
-      }
+  const auto & nexts = adjacency.at(src_ind);
+  const bool is_terminal = (std::find(nexts.begin(), nexts.end(), true) == nexts.end());
+  if (is_terminal) {
+    std::vector<size_t> path(visited_inds.begin(), visited_inds.end());
+    path.push_back(src_ind);
+    paths.emplace_back(std::move(path));
+    return;
+  }
+  for (size_t next = 0; next < nexts.size(); next++) {
+    if (!nexts.at(next)) {
+      continue;
     }
-
-    if (is_terminal) {
-      paths.push_back(current_path);
+    if (std::find(visited_inds.begin(), visited_inds.end(), next) != visited_inds.end()) {
+      // loop detected
+      std::vector<size_t> path(visited_inds.begin(), visited_inds.end());
+      path.push_back(src_ind);
+      paths.emplace_back(std::move(path));
+      continue;
     }
-
-    current_path.pop_back();
-    visited.erase(src_ind);
-  };
-
-  retrieve_paths_backward_impl(start_node);
-  return paths;
+    auto new_visited_inds = visited_inds;
+    new_visited_inds.push_back(src_ind);
+    retrievePathsBackward(adjacency, next, new_visited_inds, paths);
+  }
+  return;
 }
 
 std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
@@ -269,13 +265,20 @@ mergeLaneletsByTopologicalSort(
   }
   std::set<size_t> terminal_inds;
   for (const auto & terminal_lanelet : terminal_lanelets) {
-    terminal_inds.insert(Id2ind[terminal_lanelet.id()]);
+    if (Id2ind.count(terminal_lanelet.id()) > 0) {
+      terminal_inds.insert(Id2ind[terminal_lanelet.id()]);
+    }
   }
 
   // create adjacency matrix
   const auto n_node = lanelets.size();
-  std::vector<std::vector<bool>> adjacency(n_node, std::vector<bool>(n_node, false));
-
+  std::vector<std::vector<bool>> adjacency(n_node);
+  for (size_t dst = 0; dst < n_node; ++dst) {
+    adjacency[dst].resize(n_node);
+    for (size_t src = 0; src < n_node; ++src) {
+      adjacency[dst][src] = false;
+    }
+  }
   // NOTE: this function aims to traverse the detection lanelet in the lane direction, so if lane B
   // follows lane A on the routing_graph, adj[A][B] = true
   for (const auto & lanelet : lanelets) {
@@ -290,27 +293,33 @@ mergeLaneletsByTopologicalSort(
 
   std::unordered_map<size_t, std::vector<std::vector<size_t>>> branches;
   for (const auto & terminal_ind : terminal_inds) {
-    branches[terminal_ind] = retrievePathsBackward(adjacency, terminal_ind);
+    std::vector<std::vector<size_t>> paths;
+    std::vector<size_t> visited;
+    retrievePathsBackward(adjacency, terminal_ind, visited, paths);
+    branches[terminal_ind] = std::move(paths);
   }
 
-  for (auto & [terminal_ind, paths] : branches) {
+  for (auto it = branches.begin(); it != branches.end(); it++) {
+    auto & paths = it->second;
     for (auto & path : paths) {
       std::reverse(path.begin(), path.end());
     }
   }
   lanelet::ConstLanelets merged;
   std::vector<lanelet::ConstLanelets> originals;
   for (const auto & [ind, sub_branches] : branches) {
-    if (sub_branches.empty()) {
+    if (sub_branches.size() == 0) {
       continue;
     }
     for (const auto & sub_inds : sub_branches) {
-      lanelet::ConstLanelets to_merge;
+      lanelet::ConstLanelets to_be_merged;
+      originals.push_back(lanelet::ConstLanelets({}));
+      auto & original = originals.back();
       for (const auto & sub_ind : sub_inds) {
-        to_merge.push_back(Id2lanelet[ind2Id[sub_ind]]);
+        to_be_merged.push_back(Id2lanelet[ind2Id[sub_ind]]);
+        original.push_back(Id2lanelet[ind2Id[sub_ind]]);
       }
-      originals.push_back(to_merge);
-      merged.push_back(lanelet::utils::combineLaneletsShape(to_merge));
+      merged.push_back(lanelet::utils::combineLaneletsShape(to_be_merged));
     }
   }
   return {merged, originals};

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.hpp

@@ -110,34 +110,22 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
  * @brief this function sorts the set of lanelets topologically using topological sort and merges
  * the lanelets from each root to each end. each branch is merged and returned with the original
  * lanelets
- *
- * @param lanelets The set of input lanelets to be processed
- * @param terminal_lanelets The set of lanelets considered as terminal
- * @param routing_graph_ptr Pointer to the routing graph used for determining lanelet
- * connections
- *
- * @return A pair containing:
- *         - First: A vector of merged lanelets, where each element represents a merged lanelet from
- * a branch
- *         - Second: A vector of vectors, where each inner vector contains the original lanelets
- * that were merged to form the corresponding merged lanelet
+ * @param[in] lanelets the set of lanelets
+ * @param[in] routing_graph_ptr the routing graph
+ * @return the pair of merged lanelets and their corresponding original lanelets
  */
 std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
 mergeLaneletsByTopologicalSort(
   const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & terminal_lanelets,
   const lanelet::routing::RoutingGraphPtr routing_graph_ptr);
 
 /**
- * @brief Retrieves all paths from the given source to terminal nodes on the tree
- *
- * @param adjacency A 2D vector representing the adjacency matrix of the graph
- * @param src_ind The index of the current source node being processed
- *
- * @return A vector of vectors, where each inner vector represents a path from the source node to a
- * terminal node.
+ * @brief this functions retrieves all the paths from the given source to terminal nodes on the tree
+ @param[in] visited_inds visited node indices excluding src_ind so far
  */
-std::vector<std::vector<size_t>> retrievePathsBackward(
-  const std::vector<std::vector<bool>> & adjacency, size_t start_node);
+void retrievePathsBackward(
+  const std::vector<std::vector<bool>> & adjacency, const size_t src_ind,
+  const std::vector<size_t> & visited_inds, std::vector<std::vector<size_t>> & paths);
 
 /**
  * @brief find the index of the first point where vehicle footprint intersects with the given

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_util.cpp

@@ -42,16 +42,16 @@ TEST(TestUtil, retrievePathsBackward)
   };
   {
     const size_t src_ind = 6;
-    auto paths =
-      autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind);
+    std::vector<std::vector<size_t>> paths;
+    autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind, {}, paths);
     EXPECT_EQ(paths.size(), 1);
     EXPECT_EQ(paths.at(0).size(), 1);
     EXPECT_EQ(paths.at(0).at(0), 6);
   }
   {
     const size_t src_ind = 4;
-    auto paths =
-      autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind);
+    std::vector<std::vector<size_t>> paths;
+    autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind, {}, paths);
     EXPECT_EQ(paths.size(), 2);
     // 4 --> 6
     EXPECT_EQ(paths.at(0).size(), 2);
@@ -64,8 +64,8 @@ TEST(TestUtil, retrievePathsBackward)
   }
   {
     const size_t src_ind = 0;
-    auto paths =
-      autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind);
+    std::vector<std::vector<size_t>> paths;
+    autoware::behavior_velocity_planner::util::retrievePathsBackward(adjacency, src_ind, {}, paths);
     EXPECT_EQ(paths.size(), 3);
     // 0 --> 1 --> 2 --> 4 --> 6
     EXPECT_EQ(paths.at(0).size(), 5);