Skip to content

Commit 6cef1d5

Browse files
committed
feat(avoidance): wait and see the ambiguous stopped vehicle
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent d6ac70d commit 6cef1d5

File tree

1 file changed

+16
-0
lines changed

1 file changed

+16
-0
lines changed

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

+16
Original file line numberDiff line numberDiff line change
@@ -276,6 +276,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
276276

277277
// calculate feasible shift length based on behavior policy
278278
const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
279+
280+
if (o.is_ambiguous) {
281+
const auto prepare_distance = helper_->getMinimumPrepareDistance();
282+
const auto constant_distance = helper_->getFrontConstantDistance(o);
283+
const auto avoidance_distance = helper_->getMinAvoidanceDistance(desire_shift_length);
284+
const auto decelaration_distance =
285+
std::max(10.0, helper_->getFeasibleDecelDistance(0.0, false));
286+
if (
287+
o.longitudinal >
288+
prepare_distance + constant_distance + avoidance_distance + decelaration_distance) {
289+
o.reason = "AmbiguousStoppedVehicle(wait-and-see)";
290+
o.is_avoidable = feasible_shift_profile.has_value();
291+
continue;
292+
}
293+
}
294+
279295
if (!feasible_shift_profile.has_value()) {
280296
if (o.avoid_required && is_forward_object(o)) {
281297
break;

0 commit comments

Comments
 (0)