From 8265f9383e5e29354eff3369e34bf25cc99aae0d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 16 May 2024 19:14:36 +0900 Subject: [PATCH] fix(avoidance): unexpected road shoulder distance calculation result (#6911) * fix(avoidance): unexpected road shoulder distance calculation result Signed-off-by: satoshi-ota * fix(avoidance): prevent division by zero Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/utils.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 7ae5a97b77fff..1e83769dde599 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -912,8 +912,14 @@ double getRoadShoulderDistance( } } + const auto envelope_polygon_width = + boost::geometry::area(object.envelope_poly) / + std::max(object.length, 1e-3); // prevent division by zero + { - const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position; + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) + .position; const auto opt_intersect = tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));