diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml
index 3995866f760c6..b795d84f8f421 100644
--- a/planning/behavior_velocity_planner/package.xml
+++ b/planning/behavior_velocity_planner/package.xml
@@ -7,6 +7,7 @@
Mamoru Sobue
+ Takayuki Murooka
Satoshi Ota
diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
index 01a38f42f4d49..7485683a79176 100644
--- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -150,6 +150,33 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
}
const size_t closest_idx = closest_idx_opt.get();
+ if (stop_lines_idx_opt.has_value()) {
+ const auto stop_line_idx = stop_lines_idx_opt.value().stop_line;
+ const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line;
+ const auto keep_detection_line_idx = stop_lines_idx_opt.value().keep_detection_line;
+
+ const bool is_over_pass_judge_line =
+ util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx);
+ const bool is_before_keep_detection_line =
+ stop_lines_idx_opt.has_value()
+ ? util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx)
+ : false;
+ const bool keep_detection = is_before_keep_detection_line &&
+ std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
+
+ if (is_over_pass_judge_line && keep_detection) {
+ // in case ego could not stop exactly before the stop line, but with some overshoot,
+ // keep detection within some margin under low velocity threshold
+ } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
+ RCLCPP_INFO(logger_, "over the keep_detection line and not low speed. no plan needed.");
+ RCLCPP_DEBUG(logger_, "===== plan end =====");
+ setSafe(true);
+ setDistance(motion_utils::calcSignedArcLength(
+ path->points, planner_data_->current_pose.pose.position,
+ path->points.at(stop_line_idx).point.pose.position));
+ return true;
+ }
+ }
/* collision checking */
bool is_entry_prohibited = false;
@@ -171,8 +198,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
closest_idx, stuck_vehicle_detect_area);
/* calculate final stop lines */
- int stop_line_idx_final = -1;
- int pass_judge_line_idx_final = -1;
+ int stop_line_idx_final =
+ stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1;
+ int pass_judge_line_idx_final =
+ stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().pass_judge_line : -1;
if (external_go) {
is_entry_prohibited = false;
} else if (external_stop) {
@@ -204,29 +233,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
return false;
}
- const bool is_over_pass_judge_line =
- util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final);
- const bool is_before_keep_detection_line =
- stop_lines_idx_opt.has_value()
- ? util::isBeforeTargetIndex(
- *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line)
- : false;
- const bool keep_detection =
- is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;
-
- if (is_over_pass_judge_line && keep_detection) {
- // in case ego could not stop exactly before the stop line, but with some overshoot,
- // keep detection within some margin under low velocity threshold
- } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
- RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
- RCLCPP_DEBUG(logger_, "===== plan end =====");
- setSafe(true);
- setDistance(motion_utils::calcSignedArcLength(
- path->points, planner_data_->current_pose.pose.position,
- path->points.at(stop_line_idx_final).point.pose.position));
- return true;
- }
-
state_machine_.setStateWithMarginTime(
is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
logger_.get_child("state_machine"), *clock_);