Skip to content

Commit 59e8696

Browse files
authored
fix(intersection): judge pass judge at intersection without tl with occlusion detection (#6207)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent e67ab70 commit 59e8696

File tree

2 files changed

+36
-11
lines changed

2 files changed

+36
-11
lines changed

planning/behavior_velocity_intersection_module/src/decision_result.cpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -19,23 +19,23 @@ namespace behavior_velocity_planner::intersection
1919
std::string formatDecisionResult(const DecisionResult & decision_result)
2020
{
2121
if (std::holds_alternative<InternalError>(decision_result)) {
22-
const auto state = std::get<InternalError>(decision_result);
22+
const auto & state = std::get<InternalError>(decision_result);
2323
return "InternalError because " + state.error;
2424
}
2525
if (std::holds_alternative<OverPassJudge>(decision_result)) {
26-
const auto state = std::get<OverPassJudge>(decision_result);
26+
const auto & state = std::get<OverPassJudge>(decision_result);
2727
return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" +
2828
state.evasive_report;
2929
}
3030
if (std::holds_alternative<StuckStop>(decision_result)) {
3131
return "StuckStop";
3232
}
3333
if (std::holds_alternative<YieldStuckStop>(decision_result)) {
34-
const auto state = std::get<YieldStuckStop>(decision_result);
34+
const auto & state = std::get<YieldStuckStop>(decision_result);
3535
return "YieldStuckStop:\nsafety_report:" + state.safety_report;
3636
}
3737
if (std::holds_alternative<NonOccludedCollisionStop>(decision_result)) {
38-
const auto state = std::get<NonOccludedCollisionStop>(decision_result);
38+
const auto & state = std::get<NonOccludedCollisionStop>(decision_result);
3939
return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report;
4040
}
4141
if (std::holds_alternative<FirstWaitBeforeOcclusion>(decision_result)) {
@@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result)
4545
return "PeekingTowardOcclusion";
4646
}
4747
if (std::holds_alternative<OccludedCollisionStop>(decision_result)) {
48-
const auto state = std::get<OccludedCollisionStop>(decision_result);
48+
const auto & state = std::get<OccludedCollisionStop>(decision_result);
4949
return "OccludedCollisionStop\nsafety_report:" + state.safety_report;
5050
}
5151
if (std::holds_alternative<OccludedAbsenceTrafficLight>(decision_result)) {
52-
const auto state = std::get<OccludedAbsenceTrafficLight>(decision_result);
52+
const auto & state = std::get<OccludedAbsenceTrafficLight>(decision_result);
5353
return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report;
5454
}
5555
if (std::holds_alternative<Safe>(decision_result)) {
5656
return "Safe";
5757
}
5858
if (std::holds_alternative<FullyPrioritized>(decision_result)) {
59-
const auto state = std::get<FullyPrioritized>(decision_result);
59+
const auto & state = std::get<FullyPrioritized>(decision_result);
6060
return "FullyPrioritized\nsafety_report:" + state.safety_report;
6161
}
6262
return "";

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

+29-4
Original file line numberDiff line numberDiff line change
@@ -354,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
354354
: false;
355355
if (!has_traffic_light_) {
356356
if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) {
357-
return intersection::InternalError{
358-
"already passed maximum peeking line in the absence of traffic light"};
357+
if (has_collision) {
358+
const auto closest_idx = intersection_stoplines.closest_idx;
359+
const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis(
360+
*path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects);
361+
return intersection::OverPassJudge{
362+
"already passed maximum peeking line in the absence of traffic light.\n" +
363+
safety_report,
364+
evasive_diag};
365+
}
366+
return intersection::OverPassJudge{
367+
"already passed maximum peeking line in the absence of traffic light safely",
368+
"no evasive action required"};
359369
}
360370
return intersection::OccludedAbsenceTrafficLight{
361371
is_occlusion_cleared_with_margin,
@@ -364,7 +374,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
364374
closest_idx,
365375
first_attention_stopline_idx,
366376
occlusion_wo_tl_pass_judge_line_idx,
367-
safety_report};
377+
safety_diag};
368378
}
369379

370380
// ==========================================================================================
@@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat
12511261
return first_pass_judge_line_idx;
12521262
}();
12531263

1254-
const bool was_safe = std::holds_alternative<intersection::Safe>(prev_decision_result_);
1264+
// ==========================================================================================
1265+
// at intersection without traffic light, this module ignores occlusion even if occlusion is
1266+
// detected for real, so if collision is not detected in that context, that should be interpreted
1267+
// as "was_safe"
1268+
// ==========================================================================================
1269+
const bool was_safe = [&]() {
1270+
if (std::holds_alternative<intersection::Safe>(prev_decision_result_)) {
1271+
return true;
1272+
}
1273+
if (std::holds_alternative<intersection::OccludedAbsenceTrafficLight>(prev_decision_result_)) {
1274+
const auto & state =
1275+
std::get<intersection::OccludedAbsenceTrafficLight>(prev_decision_result_);
1276+
return !state.collision_detected;
1277+
}
1278+
return false;
1279+
}();
12551280

12561281
const bool is_over_1st_pass_judge_line =
12571282
util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx);

0 commit comments

Comments
 (0)