feat(intersection): fix topological sort for complicated intersection

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CMakeLists.txt

@@ -28,6 +28,16 @@ target_link_libraries(${PROJECT_NAME}
 
 ament_auto_package(INSTALL_TO_SHARE config)
 
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  file(GLOB_RECURSE TEST_SOURCES test/*.cpp)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    ${TEST_SOURCES}
+  )
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
+endif()
+
 install(PROGRAMS
   scripts/ttc.py
   DESTINATION lib/${PROJECT_NAME}

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml

@@ -26,6 +26,7 @@
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_route_handler</depend>
   <depend>autoware_rtc_interface</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>fmt</depend>
@@ -40,6 +41,7 @@
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -590,7 +590,8 @@ class IntersectionModule : public SceneModuleInterface
    * @brief generate discretized detection lane linestring.
    */
   std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
-    lanelet::ConstLanelets detection_lanelets_all,
+    const lanelet::ConstLanelets & occlusion_detection_lanelets,
+    const lanelet::ConstLanelets & conflicting_detection_lanelets,
     const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
   /** @} */
 

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

@@ -1,4 +1,4 @@
 // Copyright 2024 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -266,8 +266,8 @@
 
   if (!occlusion_attention_divisions_) {
     occlusion_attention_divisions_ = generateDetectionLaneDivisions(
-      intersection_lanelets.occlusion_attention(), routing_graph_ptr,
-      planner_data_->occupancy_grid->info.resolution);
+      intersection_lanelets.occlusion_attention(), intersection_lanelets.attention_non_preceding(),
+      routing_graph_ptr, planner_data_->occupancy_grid->info.resolution);
   }
 
   if (has_traffic_light_) {
@@ -577,6 +577,73 @@
   return intersection_stoplines;
 }
 
+static std::vector<std::deque<lanelet::ConstLanelet>> getPrecedingLaneletsUptoIntersectionRecursive(
+  const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length, const lanelet::ConstLanelets & exclude_lanelets)
+{
+  std::vector<std::deque<lanelet::ConstLanelet>> preceding_lanelet_sequences;
+
+  const auto prev_lanelets = graph->previous(lanelet);
+  const double lanelet_length = lanelet::utils::getLaneletLength3d(lanelet);
+
+  // end condition of the recursive function
+  if (prev_lanelets.empty() || lanelet_length >= length) {
+    preceding_lanelet_sequences.push_back({lanelet});
+    return preceding_lanelet_sequences;
+  }
+
+  for (const auto & prev_lanelet : prev_lanelets) {
+    if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
+      // if prev_lanelet is included in exclude_lanelets,
+      // remove prev_lanelet from preceding_lanelet_sequences
+      continue;
+    }
+    if (const std::string turn_direction = prev_lanelet.attributeOr("turn_direction", "else");
+        turn_direction == "left" || turn_direction == "right") {
+      continue;
+    }
+
+    // get lanelet sequence after prev_lanelet
+    auto tmp_lanelet_sequences = getPrecedingLaneletsUptoIntersectionRecursive(
+      graph, prev_lanelet, length - lanelet_length, exclude_lanelets);
+    for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
+      tmp_lanelet_sequence.push_back(lanelet);
+      preceding_lanelet_sequences.push_back(tmp_lanelet_sequence);
+    }
+  }
+
+  if (preceding_lanelet_sequences.empty()) {
+    preceding_lanelet_sequences.push_back({lanelet});
+  }
+  return preceding_lanelet_sequences;
+}
+
+static std::vector<lanelet::ConstLanelets> getPrecedingLaneletsUptoIntersection(
+  const lanelet::routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
+  const double length, const lanelet::ConstLanelets & exclude_lanelets)
+{
+  std::vector<lanelet::ConstLanelets> lanelet_sequences_vec;
+  const auto prev_lanelets = graph->previous(lanelet);
+  for (const auto & prev_lanelet : prev_lanelets) {
+    if (lanelet::utils::contains(exclude_lanelets, prev_lanelet)) {
+      // if prev_lanelet is included in exclude_lanelets,
+      // remove prev_lanelet from preceding_lanelet_sequences
+      continue;
+    }
+    if (const std::string turn_direction = prev_lanelet.attributeOr("turn_direction", "else");
+        turn_direction == "left" || turn_direction == "right") {
+      continue;
+    }
+    // convert deque into vector
+    const auto lanelet_sequences_deq =
+      getPrecedingLaneletsUptoIntersectionRecursive(graph, prev_lanelet, length, exclude_lanelets);
+    for (const auto & lanelet_sequence : lanelet_sequences_deq) {
+      lanelet_sequences_vec.emplace_back(lanelet_sequence.begin(), lanelet_sequence.end());
+    }
+  }
+  return lanelet_sequences_vec;
+}
+
 IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const lanelet::ConstLanelet assigned_lanelet) const
@@ -706,66 +773,58 @@
       if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(ll);
       // get preceding lanelets without ego_lanelets
       // to prevent the detection area from including the ego lanes and its' preceding lanes.
-      const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences(
-        routing_graph_ptr, ll, length, ego_lanelets);
+      const auto lanelet_sequences =
+        getPrecedingLaneletsUptoIntersection(routing_graph_ptr, ll, length, ego_lanelets);
       for (const auto & ls : lanelet_sequences) {
         for (const auto & l : ls) {
           const auto & inner_inserted = detection_ids.insert(l.id());
           if (inner_inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l);
         }
       }
     }
   }
-  lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction;
-  for (const auto & ll : occlusion_detection_and_preceding_lanelets) {
-    const std::string turn_direction = ll.attributeOr("turn_direction", "else");
-    if (turn_direction == "left" || turn_direction == "right") {
-      continue;
-    }
-    occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll);
-  }
 
   auto [attention_lanelets, original_attention_lanelet_sequences] =
-    util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr);
+    util::mergeLaneletsByTopologicalSort(
+      detection_and_preceding_lanelets, detection_lanelets, routing_graph_ptr);
 
   IntersectionLanelets result;
   result.attention_ = std::move(attention_lanelets);
   for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) {
     // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that
     // back() exists.
     std::optional<lanelet::ConstLineString3d> stopline{std::nullopt};
     for (auto it = original_attention_lanelet_seq.rbegin();
          it != original_attention_lanelet_seq.rend(); ++it) {
       const auto traffic_lights = it->regulatoryElementsAs<lanelet::TrafficLight>();
       for (const auto & traffic_light : traffic_lights) {
         const auto stopline_opt = traffic_light->stopLine();
         if (!stopline_opt) continue;
         stopline = stopline_opt.get();
         break;
       }
       if (stopline) break;
     }
     result.attention_stoplines_.push_back(stopline);
   }
   result.attention_non_preceding_ = std::move(detection_lanelets);
   for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) {
     std::optional<lanelet::ConstLineString3d> stopline = std::nullopt;
     const auto & ll = result.attention_non_preceding_.at(i);
     const auto traffic_lights = ll.regulatoryElementsAs<lanelet::TrafficLight>();
     for (const auto & traffic_light : traffic_lights) {
       const auto stopline_opt = traffic_light->stopLine();
       if (!stopline_opt) continue;
       stopline = stopline_opt.get();
     }
     result.attention_non_preceding_stoplines_.push_back(stopline);
   }
   result.conflicting_ = std::move(conflicting_ex_ego_lanelets);
   result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_);
   // NOTE: occlusion_attention is not inverted here
   // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and
   // then trim part of them based on curvature threshold
-  result.occlusion_attention_ =
-    std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction);
+  result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets);
 
   // NOTE: to properly update(), each element in conflicting_/conflicting_area_,
   // attention_non_preceding_/attention_non_preceding_area_ need to be matched
@@ -851,7 +910,8 @@
 }
 
 std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLaneDivisions(
-  lanelet::ConstLanelets detection_lanelets_all,
+  const lanelet::ConstLanelets & occlusion_detection_lanelets,
+  const lanelet::ConstLanelets & conflicting_detection_lanelets,
   const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const
 {
   const double curvature_threshold =
@@ -861,9 +921,9 @@
 
   using lanelet::utils::getCenterlineWithOffset;
 
-  // (0) remove left/right lanelet
+  // (0) remove curved
   lanelet::ConstLanelets detection_lanelets;
-  for (const auto & detection_lanelet : detection_lanelets_all) {
+  for (const auto & detection_lanelet : occlusion_detection_lanelets) {
     // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet
     const auto fine_centerline =
       lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds);
@@ -875,8 +935,8 @@
   }
 
   // (1) tsort detection_lanelets
-  const auto [merged_detection_lanelets, originals] =
-    util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr);
+  const auto [merged_detection_lanelets, originals] = util::mergeLaneletsByTopologicalSort(
+    detection_lanelets, conflicting_detection_lanelets, routing_graph_ptr);
 
   // (2) merge each branch to one lanelet
   // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -32,12 +32,10 @@
 
 #include <algorithm>
 #include <cmath>
-#include <limits>
-#include <map>
-#include <memory>
 #include <set>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 namespace autoware::behavior_velocity_planner::util
@@ -214,98 +212,106 @@
   }
   return std::nullopt;
 }
+std::vector<std::vector<size_t>> retrievePathsBackward(
+  const std::vector<std::vector<bool>> & adjacency, size_t start_node)
+{
+  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);
+        }
+      }
+    }
+
+    if (is_terminal) {
+      paths.push_back(current_path);
+    }
+
+    current_path.pop_back();
+    visited.erase(src_ind);
+  };
+
+  retrieve_paths_backward_impl(start_node);
+  return paths;
+}
 
 std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
 mergeLaneletsByTopologicalSort(
-  const lanelet::ConstLanelets & lanelets,
+  const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & terminal_lanelets,
   const lanelet::routing::RoutingGraphPtr routing_graph_ptr)
 {
-  const int n_node = lanelets.size();
-  std::vector<std::vector<int>> adjacency(n_node);
-  for (int dst = 0; dst < n_node; ++dst) {
-    adjacency[dst].resize(n_node);
-    for (int src = 0; src < n_node; ++src) {
-      adjacency[dst][src] = false;
-    }
-  }
-  std::set<lanelet::Id> lanelet_ids;
-  std::unordered_map<lanelet::Id, int> id2ind;
-  std::unordered_map<int, lanelet::Id> ind2id;
-  std::unordered_map<lanelet::Id, lanelet::ConstLanelet> id2lanelet;
-  int ind = 0;
+  std::set<lanelet::Id> lanelet_Ids;
+  std::unordered_map<lanelet::Id, size_t> Id2ind;
+  std::unordered_map<size_t, lanelet::Id> ind2Id;
+  std::unordered_map<lanelet::Id, lanelet::ConstLanelet> Id2lanelet;
   for (const auto & lanelet : lanelets) {
-    lanelet_ids.insert(lanelet.id());
-    const auto id = lanelet.id();
-    id2ind[id] = ind;
-    ind2id[ind] = id;
-    id2lanelet[id] = lanelet;
-    ind++;
+    size_t ind = ind2Id.size();
+    const auto Id = lanelet.id();
+    lanelet_Ids.insert(Id);
+    Id2ind[Id] = ind;
+    ind2Id[ind] = Id;
+    Id2lanelet[Id] = lanelet;
   }
-  // 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
+  std::set<size_t> terminal_inds;
+  for (const auto & terminal_lanelet : terminal_lanelets) {
+    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));
+
+  // 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) {
     const auto & followings = routing_graph_ptr->following(lanelet);
-    const int dst = lanelet.id();
+    const auto src = lanelet.id();
     for (const auto & following : followings) {
-      if (const int src = following.id(); lanelet_ids.find(src) != lanelet_ids.end()) {
-        adjacency[(id2ind[src])][(id2ind[dst])] = true;
+      if (const auto dst = following.id(); lanelet_Ids.find(dst) != lanelet_Ids.end()) {
+        adjacency[(Id2ind[dst])][(Id2ind[src])] = true;
       }
     }
   }
-  // terminal node
-  std::map<lanelet::Id, std::vector<lanelet::Id>> branches;
-  auto has_no_previous = [&](const int node) {
-    for (int dst = 0; dst < n_node; dst++) {
-      if (adjacency[dst][node]) {
-        return false;
-      }
-    }
-    return true;
-  };
-  for (int src = 0; src < n_node; src++) {
-    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"(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)];
-    }
+
+  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);
   }
-  for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) {
-    auto & branch = it->second;
-    std::reverse(branch.begin(), branch.end());
+
+  for (auto & [terminal_ind, paths] : branches) {
+    for (auto & path : paths) {
+      std::reverse(path.begin(), path.end());
+    }
   }
   lanelet::ConstLanelets merged;
   std::vector<lanelet::ConstLanelets> originals;
-  for (const auto & [id, sub_ids] : branches) {
-    if (sub_ids.size() == 0) {
+  for (const auto & [ind, sub_branches] : branches) {
+    if (sub_branches.empty()) {
       continue;
     }
-    lanelet::ConstLanelets merge;
-    originals.push_back(lanelet::ConstLanelets({}));
-    auto & original = originals.back();
-    for (const auto sub_id : sub_ids) {
-      merge.push_back(id2lanelet[sub_id]);
-      original.push_back(id2lanelet[sub_id]);
+    for (const auto & sub_inds : sub_branches) {
+      lanelet::ConstLanelets to_merge;
+      for (const auto & sub_ind : sub_inds) {
+        to_merge.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(merge));
   }
   return {merged, originals};
 }