Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(intersection): judge pass judge at intersection without tl with occlusion detection #6207

Merged
merged 1 commit into from
Jan 29, 2024
Merged

fix(intersection): judge pass judge at intersection without tl with occlusion detection #6207

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions planning/behavior_velocity_intersection_module/src/decision_result.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,23 +19,23 @@ namespace behavior_velocity_planner::intersection
std::string formatDecisionResult(const DecisionResult & decision_result)
{
if (std::holds_alternative<InternalError>(decision_result)) {
const auto state = std::get<InternalError>(decision_result);
const auto & state = std::get<InternalError>(decision_result);
return "InternalError because " + state.error;
}
if (std::holds_alternative<OverPassJudge>(decision_result)) {
const auto state = std::get<OverPassJudge>(decision_result);
const auto & state = std::get<OverPassJudge>(decision_result);
return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" +
state.evasive_report;
}
if (std::holds_alternative<StuckStop>(decision_result)) {
return "StuckStop";
}
if (std::holds_alternative<YieldStuckStop>(decision_result)) {
const auto state = std::get<YieldStuckStop>(decision_result);
const auto & state = std::get<YieldStuckStop>(decision_result);
return "YieldStuckStop:\nsafety_report:" + state.safety_report;
}
if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
const auto state = std::get<NonOccludedCollisionStop>(decision_result);
const auto & state = std::get<NonOccludedCollisionStop>(decision_result);
return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report;
}
if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
Expand All @@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result)
return "PeekingTowardOcclusion";
}
if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
const auto state = std::get<OccludedCollisionStop>(decision_result);
const auto & state = std::get<OccludedCollisionStop>(decision_result);
return "OccludedCollisionStop\nsafety_report:" + state.safety_report;
}
if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
const auto state = std::get<OccludedAbsenceTrafficLight>(decision_result);
const auto & state = std::get<OccludedAbsenceTrafficLight>(decision_result);
return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report;
}
if (std::holds_alternative<Safe>(decision_result)) {
return "Safe";
}
if (std::holds_alternative<FullyPrioritized>(decision_result)) {
const auto state = std::get<FullyPrioritized>(decision_result);
const auto & state = std::get<FullyPrioritized>(decision_result);
return "FullyPrioritized\nsafety_report:" + state.safety_report;
}
return "";
Expand Down
33 changes: 29 additions & 4 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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};
}

// ==========================================================================================
Expand Down Expand Up @@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
return first_pass_judge_line_idx;
}();

const bool was_safe = std::holds_alternative<intersection::Safe>(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<intersection::Safe>(prev_decision_result_)) {
return true;
}
if (std::holds_alternative<intersection::OccludedAbsenceTrafficLight>(prev_decision_result_)) {
const auto & state =
std::get<intersection::OccludedAbsenceTrafficLight>(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);
Expand Down