Skip to content

Commit 3440c30

Browse files
kaigohiraokosuke55
authored andcommitted
Revert "fix(no_stopping_area): fix stopping in front of a no stopping area in parking on the shoulder (autowarefoundation#6517)"
This reverts commit 144b9c7.
1 parent cb557db commit 3440c30

File tree

1 file changed

+3
-6
lines changed

1 file changed

+3
-6
lines changed

planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

+3-6
Original file line numberDiff line numberDiff line change
@@ -317,7 +317,9 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
317317
}
318318
++ego_area_start_idx;
319319
}
320-
320+
if (ego_area_start_idx > num_ignore_nearest) {
321+
ego_area_start_idx--;
322+
}
321323
if (!is_in_area) {
322324
return ego_area;
323325
}
@@ -329,11 +331,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
329331
const auto & p = pp.at(i).point.pose.position;
330332
if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) {
331333
dist_from_area_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1));
332-
333-
// do not take extra distance and exit as soon as p is outside no stopping area
334-
// just a temporary fix
335-
ego_area_end_idx = i - 1;
336-
break;
337334
}
338335
if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) {
339336
break;

0 commit comments

Comments
 (0)