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

…ut traffic light when occlusion feature is off Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Oct 14, 2023 · aec283e · aec283e
1 parent 4383d6b
commit aec283e
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 5 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -978,7 +978,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
@@ -269,7 +269,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;
@@ -419,8 +419,8 @@ 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;
   }

diff --git 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(