fix(intersection): consider braking distance for pass judge

Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Oct 16, 2023 · f367f51 · f367f51
1 parent 8ed98a9
commit f367f51
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -1079,13 +1079,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   const bool keep_detection =
     (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr);
   const bool was_safe = std::holds_alternative<IntersectionModule::Safe>(prev_decision_result_);
+  const bool enable_sudden_stop = true;
   // if ego is over the pass judge line and not stopped
   if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) {
     RCLCPP_DEBUG(
       logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection");
     // do nothing
   } else if (
-    (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) ||
+    (was_safe && (is_over_default_stop_line || enable_sudden_stop) && is_over_pass_judge_line &&
+     is_go_out_) ||
     is_permanent_go_) {
     // is_go_out_: previous RTC approval
     // activated_: current RTC approval

diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -415,12 +415,14 @@ std::optional<IntersectionStopLines> generateIntersectionStopLines(
     intersection_stop_lines_temp.occlusion_peeking_stop_line =
       intersection_stop_lines_temp.default_stop_line;
   }
+  /*
   if (
     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;
   }
+  */
 
   IntersectionStopLines intersection_stop_lines;
   intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx;