do not plan if detection area is not empty and stop line idx is 0

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
autowarefoundation · Jun 27, 2023 · c49ec98 · c49ec98
1 parent 3fea24f
commit c49ec98
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -715,6 +715,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     return IntersectionModule::Indecisive{};
   }
 
+  if (default_stop_line_idx == 0) {
+    RCLCPP_DEBUG(logger_, "stop line index is 0");
+    return IntersectionModule::Indecisive{};
+  };
+
   const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   debug_data_.pass_judge_wall_pose =
     planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path);
@@ -795,7 +800,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
       path->points.at(default_stop_line_idx).point.pose.position);
     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();
+    if (over_stop_line) {
+      before_creep_state_machine_.setState(StateMachine::State::GO);
+    }
     if (before_creep_state_machine_.getState() == StateMachine::State::GO) {
       if (has_collision) {
         return IntersectionModule::OccludedCollisionStop{