fix(no_stopping_area): skip drawing a stop line in the case where the detected stop lines are goals

planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -251,6 +251,10 @@
   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 @@
     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,23 +329,20 @@
     }
     ++ego_area_start_idx;
   }
-
+  if (ego_area_start_idx > num_ignore_nearest) {
+    ego_area_start_idx--;
+  }
   if (!is_in_area) {
     return ego_area;
   }
   double dist_from_area_sum = 0.0;
   // decide end idx with extract distance
   size_t ego_area_end_idx = ego_area_start_idx;
   for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) {
     dist_from_start_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1));
     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;