fix(no_stopping_area): skip drawing a stop line in the case where the…

… detected stop lines are goals (autowarefoundation#7280)

* 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.

* 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 <[email protected]>
Co-authored-by: kosuke55 <[email protected]>
Signed-off-by: Simon Eisenmann <[email protected]>

planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -251,6 +251,10 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea(
   const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly)
 {
   const double stop_vel = std::numeric_limits<float>::min();
+
+  // if the detected stop point is near goal, it's ignored.
+  static constexpr double close_to_goal_distance = 1.0;
+
   // stuck points by stop line
   for (size_t i = 0; i < path.points.size() - 1; ++i) {
     const auto p0 = path.points.at(i).point.pose.position;
@@ -260,6 +264,14 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea(
     if (v0 > stop_vel && v1 > stop_vel) {
       continue;
     }
+    // judge if stop point p0 is near goal, by its distance to the path end.
+    const double dist_to_path_end =
+      motion_utils::calcSignedArcLength(path.points, i, path.points.size() - 1);
+    if (dist_to_path_end < close_to_goal_distance) {
+      // exit with false, cause position is near goal.
+      return false;
+    }
+
     const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}};
     std::vector<Point2d> collision_points;
     bg::intersection(poly, line, collision_points);
@@ -317,7 +329,9 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
     }
     ++ego_area_start_idx;
   }
-
+  if (ego_area_start_idx > num_ignore_nearest) {
+    ego_area_start_idx--;
+  }
   if (!is_in_area) {
     return ego_area;
   }
@@ -329,11 +343,6 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon(
     const auto & p = pp.at(i).point.pose.position;
     if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) {
       dist_from_area_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1));
-
-      // do not take extra distance and exit as soon as p is outside no stopping area
-      // just a temporary fix
-      ego_area_end_idx = i - 1;
-      break;
     }
     if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) {
       break;