fix(avoidance): discard envelope polygon if the objects move long dis…

…tance

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -679,26 +679,39 @@ void fillObjectEnvelopePolygon(
     return;
   }
 
-  const auto envelope_poly =
+  const auto one_shot_envelope_poly =
     createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
 
-  if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) {
+  if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) {
     object_data.envelope_poly = same_id_obj->envelope_poly;
     return;
   }
 
   std::vector<Polygon2d> unions;
-  boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions);
+  boost::geometry::union_(one_shot_envelope_poly, same_id_obj->envelope_poly, unions);
 
   if (unions.empty()) {
-    object_data.envelope_poly =
-      createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
+    object_data.envelope_poly = one_shot_envelope_poly;
     return;
   }
 
   boost::geometry::correct(unions.front());
 
-  object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0);
+  const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0);
+
+  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object);
+  const auto object_polygon_area = boost::geometry::area(object_polygon);
+  const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly);
+
+  // keep multi-step envelope polygon.
+  constexpr double THRESHOLD = 5.0;
+  if (envelope_polygon_area < object_polygon_area * THRESHOLD) {
+    object_data.envelope_poly = multi_step_envelope_poly;
+    return;
+  }
+
+  // use latest one-shot envelope polygon.
+  object_data.envelope_poly = one_shot_envelope_poly;
 }
 
 void fillObjectMovingTime(