From 326e2a9f5cf6dceeb6ef7c9e2bdf5eec89edf411 Mon Sep 17 00:00:00 2001 From: sbtjagu <145494402+sbtjagu@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:02:12 +0200 Subject: [PATCH] ackermann: add protection against float precision problem in acceptance radius update (#23478) * ackermann: add protection against float precision problem in acceptance radius update * ackermann: protect against divide-by-zero --------- Co-authored-by: Mathieu Bresciani --- .../RoverAckermannGuidance/RoverAckermannGuidance.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index db0495b67a..ade89f6c7e 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -268,8 +268,9 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) { - const float theta = acosf((curr_to_prev_wp_ned * curr_to_next_wp_ned) / (curr_to_prev_wp_ned.norm() * - curr_to_next_wp_ned.norm())) / 2.f; + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + const float theta = acosf(cosin) / 2.f; const float min_turning_radius = wheel_base / sinf(max_steer_angle); const float acceptance_radius_temp = min_turning_radius / tanf(theta); const float acceptance_radius_temp_scaled = acceptance_radius_gain *