Merge pull request #1482 from tier4/cherry-pick/pr8561

fix(intersection): cherry pick/pr8561

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

@@ -24,6 +24,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>
@@ -39,6 +40,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,
+    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

@@ -266,8 +266,8 @@ Result<IntersectionModule::BasicData, InternalError> IntersectionModule::prepare
 
   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 @@ std::optional<IntersectionStopLines> IntersectionModule::generateIntersectionSto
   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,8 +773,8 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
       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 & inserted = detection_ids.insert(l.id());
@@ -716,17 +783,10 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
       }
     }
   }
-  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);
@@ -764,8 +824,7 @@ IntersectionLanelets IntersectionModule::generateObjectiveLanelets(
   // 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::optional<PathLanelets> IntersectionModule::generatePathLanelets(
 }
 
 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 @@ std::vector<lanelet::ConstLineString3d> IntersectionModule::generateDetectionLan
 
   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);
@@ -874,9 +934,17 @@ 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, 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
@@ -892,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;