From a06110de62be78e8a104f04371eb012471a8c5dd Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 20 Oct 2023 18:14:17 +0900 Subject: [PATCH] fix(avoidance): discard envelope polygon if the objects move long distance Signed-off-by: satoshi-ota --- .../src/utils/avoidance/utils.cpp | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index a9f40dad86ae4..a0d11e8421c86 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/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 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(