added COLLISION_DETECTED state

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
soblin · May 22, 2023 · d262546 · d262546
1 parent 7fd41ab
commit d262546
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -313,24 +313,32 @@ 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 && !has_collision) {
+    } else if (has_collision) {
+      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 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 {
-      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(

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(