feat(intersection): disable peeking while collision is detected (auto…

…warefoundation#3765) * feat(intersection): disable peeking while collision is detected Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * preempt PR3711 (#11) Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed !has_collision Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * Preempt PR3711(2) (#13) * fix(intersection): use 0 offset peeking line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use braking distance Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * add pass_judge_wall_pose marker Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * keep detection if ego is between stopline and pass judge line and velocity is below threshold Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * modify condition Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use 0.0s for delay response time Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * right turn scenario passes Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * find all stop line candidates at first in order Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use keep_detection_vel for staticPassJudgeLine Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * added COLLISION_DETECTED state Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * po Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * Revert "Preempt PR3711(2) (#13)" This reverts commit 7fd41ab. Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * Revert "preempt PR3711 (#11)" This reverts commit a6d7fb6. Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * call setSafe() regardless of occlusion_stop_required Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
h-ohta · May 23, 2023 · 9fd54a2 · 9fd54a2
1 parent 0c65a3f
commit 9fd54a2
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 16 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -303,24 +303,34 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   is_actually_occluded_ = !is_occlusion_cleared;
   is_forcefully_occluded_ = ext_occlusion_requested;
   if (!is_occlusion_cleared || ext_occlusion_requested) {
+    const bool approached_stop_line =
+      (std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin);
+    const bool is_stopped = planner_data_->isVehicleStopped();
     if (!default_stop_line_idx_opt) {
       RCLCPP_DEBUG(logger_, "occlusion is detected but default stop line is not set or generated");
       RCLCPP_DEBUG(logger_, "===== plan end =====");
       return true;
-    }
-    if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
-      occlusion_stop_required = true;
-      occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
-
-      // clear first stop line
-      // insert creep velocity [closest_idx, occlusion_stop_line)
-      insert_creep_during_occlusion =
-        std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
-      occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
+    } else if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
+      if (!has_collision) {
+        occlusion_stop_required = true;
+        occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
+        // clear first stop line
+        // insert creep velocity [closest_idx, occlusion_stop_line)
+        insert_creep_during_occlusion =
+          std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
+        occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE;
+      } else {
+        collision_stop_required = true;
+        stop_line_idx = default_stop_line_idx_opt;
+        occlusion_stop_required = true;
+        occlusion_peeking_line_idx = occlusion_peeking_line_idx_opt;
+        // clear first stop line
+        // insert creep velocity [closest_idx, occlusion_stop_line)
+        insert_creep_during_occlusion =
+          std::make_pair(closest_idx, occlusion_peeking_line_idx_opt.value());
+        occlusion_state_ = OcclusionState::COLLISION_DETECTED;
+      }
     } else {
-      const bool approached_stop_line =
-        (std::fabs(dist_1st_stopline) < planner_param_.common.stop_overshoot_margin);
-      const bool is_stopped = planner_data_->isVehicleStopped();
       if (is_stopped && approached_stop_line) {
         // start waiting at the first stop line
         before_creep_state_machine_.setStateWithMarginTime(
@@ -403,10 +413,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   if (occlusion_stop_required) {
     occlusion_first_stop_required_ = first_phase_stop_required;
     occlusion_safety_ = is_occlusion_cleared;
-  } else {
-    /* collision */
-    setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
   }
+  /* collision */
+  setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
 
   /* make decision */
   const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -131,6 +131,7 @@ class IntersectionModule : public SceneModuleInterface
     WAIT_FIRST_STOP_LINE,
     CREEP_SECOND_STOP_LINE,
     CLEARED,
+    COLLISION_DETECTED,
   };
 
   IntersectionModule(