Skip to content

Commit fbd1ecc

Browse files
kaigohiraopre-commit-ci[bot]shmpwkkosuke55
authored and
KhalilSelyan
committed
fix(no_stopping_area): skip drawing a stop line in the case where the detected stop lines are goals (#7280)
* Revert "fix(no_stopping_area): fix stopping in front of a no stopping area in parking on the shoulder (#6517)" This reverts commit 144b9c7. * if the stop point is near goal(distance < 1m), no stopping area module does not insert stop line * style(pre-commit): autofix * Update planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 0d4ca6d commit fbd1ecc

File tree

1 file changed

+15
-6
lines changed

1 file changed

+15
-6
lines changed

planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

+15-6
Original file line numberDiff line numberDiff line change
@@ -251,6 +251,10 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea(
251251
const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly)
252252
{
253253
const double stop_vel = std::numeric_limits<float>::min();
254+
255+
// if the detected stop point is near goal, it's ignored.
256+
static constexpr double close_to_goal_distance = 1.0;
257+
254258
// stuck points by stop line
255259
for (size_t i = 0; i < path.points.size() - 1; ++i) {
256260
const auto p0 = path.points.at(i).point.pose.position;
@@ -260,6 +264,14 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea(
260264
if (v0 > stop_vel && v1 > stop_vel) {
261265
continue;
262266
}
267+
// judge if stop point p0 is near goal, by its distance to the path end.
268+
const double dist_to_path_end =
269+
motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1);
270+
if (dist_to_path_end < close_to_goal_distance) {
271+
// exit with false, cause position is near goal.
272+
return false;
273+
}
274+
263275
const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}};
264276
std::vector<Point2d> collision_points;
265277
bg::intersection(poly, line, collision_points);
@@ -317,7 +329,9 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
317329
}
318330
++ego_area_start_idx;
319331
}
320-
332+
if (ego_area_start_idx > num_ignore_nearest) {
333+
ego_area_start_idx--;
334+
}
321335
if (!is_in_area) {
322336
return ego_area;
323337
}
@@ -329,11 +343,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
329343
const auto & p = pp.at(i).point.pose.position;
330344
if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) {
331345
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;
337346
}
338347
if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) {
339348
break;

0 commit comments

Comments
 (0)