fix(intersection): prevent unnecessary stop inside intersection witho…

…ut traffic light when occlusion feature is off

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

diff.patch

@@ -0,0 +1,56 @@
+diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+index 75bb4e861a..f394b70433 100644
+--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
++++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+@@ -900,7 +900,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
+   const auto intersection_stop_lines_opt = util::generateIntersectionStopLines(
+     first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info,
+     planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin,
+-    peeking_offset, path);
++    peeking_offset, planner_param_.occlusion.enable, path);
+   if (!intersection_stop_lines_opt) {
+     return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"};
+   }
+diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
+index 722bf696ef..b43e4f01f3 100644
+--- a/planning/behavior_velocity_intersection_module/src/util.cpp
++++ b/planning/behavior_velocity_intersection_module/src/util.cpp
+@@ -222,7 +222,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
+   const lanelet::CompoundPolygon3d & first_detection_area,
+   const std::shared_ptr<const PlannerData> & planner_data,
+   const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline,
+-  const double stop_line_margin, const double peeking_offset,
++  const double stop_line_margin, const double peeking_offset, const double enable_occlusion,
+   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path)
+ {
+   const auto & path_ip = interpolated_path_info.path;
+@@ -371,10 +371,14 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
+       intersection_stop_lines_temp.default_stop_line;
+   }
+   if (
+-    intersection_stop_lines_temp.occlusion_peeking_stop_line >
+-    intersection_stop_lines_temp.pass_judge_line) {
++    enable_occlusion && intersection_stop_lines_temp.occlusion_peeking_stop_line >
++                          intersection_stop_lines_temp.pass_judge_line) {
+     intersection_stop_lines_temp.pass_judge_line =
+       intersection_stop_lines_temp.occlusion_peeking_stop_line;
++    RCLCPP_INFO(
++      rclcpp::get_logger("temp"),
++      "intersection_stop_lines_temp.occlusion_peeking_stop_line > "
++      "intersection_stop_lines_temp.pass_judge_line");
+   }
+
+   IntersectionStopLines intersection_stop_lines;
+diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
+index a75545353f..0eb540fabb 100644
+--- a/planning/behavior_velocity_intersection_module/src/util.hpp
++++ b/planning/behavior_velocity_intersection_module/src/util.hpp
+@@ -70,7 +70,7 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
+   const lanelet::CompoundPolygon3d & first_detection_area,
+   const std::shared_ptr<const PlannerData> & planner_data,
+   const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline,
+-  const double stop_line_margin, const double peeking_offset,
++  const double stop_line_margin, const double peeking_offset, const double enable_occlusion,
+   autoware_auto_planning_msgs::msg::PathWithLaneId * original_path);
+
+ std::optional<size_t> getFirstPointInsidePolygon(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -928,7 +928,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const auto intersection_stop_lines_opt = util::generateIntersectionStopLines(
     first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info,
     planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin,
-    peeking_offset, path);
+    peeking_offset, planner_param_.occlusion.enable, path);
   if (!intersection_stop_lines_opt) {
     return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"};
   }