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_);