diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index 93a7c2a29d2f7..aefd59a72f9b4 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -19,11 +19,11 @@ namespace behavior_velocity_planner::intersection std::string formatDecisionResult(const DecisionResult & decision_result) { if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "InternalError because " + state.error; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + state.evasive_report; } @@ -31,11 +31,11 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "StuckStop"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "YieldStuckStop:\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { @@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "PeekingTowardOcclusion"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { return "Safe"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "FullyPrioritized\nsafety_report:" + state.safety_report; } return ""; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75e2da034780a..3f6136673a44a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -354,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::InternalError{ - "already passed maximum peeking line in the absence of traffic light"}; + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light.\n" + + safety_report, + evasive_diag}; + } + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light safely", + "no evasive action required"}; } return intersection::OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, @@ -364,7 +374,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_report}; + safety_diag}; } // ========================================================================================== @@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + // ========================================================================================== + // at intersection without traffic light, this module ignores occlusion even if occlusion is + // detected for real, so if collision is not detected in that context, that should be interpreted + // as "was_safe" + // ========================================================================================== + const bool was_safe = [&]() { + if (std::holds_alternative(prev_decision_result_)) { + return true; + } + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = + std::get(prev_decision_result_); + return !state.collision_detected; + } + return false; + }(); const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx);