From 9628276d68807fd90a1bfc6d304776eabb7e7cc1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 31 Oct 2023 18:25:53 +0900 Subject: [PATCH] fix(avoidance): discard envelope polygon if the objects move long distance (#5388) * fix(avoidance): discard envelope polygon if the objects move long distance Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/src/utils/avoidance/utils.cpp Co-authored-by: Kyoichi Sugahara * Update planning/behavior_path_planner/src/utils/avoidance/utils.cpp Co-authored-by: Kyoichi Sugahara --------- Signed-off-by: satoshi-ota Co-authored-by: Kyoichi Sugahara --- .../src/utils/avoidance/utils.cpp | 27 ++++++++++++++----- 1 file changed, 21 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 54a1c0c649916..2e441907b6d19 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -754,26 +754,41 @@ 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 the one_shot_envelope_poly is within the registered envelope, use the registered one + 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 union fails, use the current envelope 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(