fix(intersection): refactor first stop timeout (autowarefoundation#4457)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
tier4 · Aug 17, 2023 · bb14a40 · bb14a40
1 parent e9dfe5c
commit bb14a40
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -895,7 +895,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     const bool approached_stop_line =
       (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin);
     const bool over_stop_line = (dist_stopline < 0.0);
-    const bool is_stopped = planner_data_->isVehicleStopped();
+    const bool is_stopped =
+      planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time);
     if (over_stop_line) {
       before_creep_state_machine_.setState(StateMachine::State::GO);
     }
@@ -913,8 +914,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     } else {
       if (is_stopped && approached_stop_line) {
         // start waiting at the first stop line
-        before_creep_state_machine_.setStateWithMarginTime(
-          StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_);
+        before_creep_state_machine_.setState(StateMachine::State::GO);
       }
       is_peeking_ = true;
       return IntersectionModule::FirstWaitBeforeOcclusion{