From 195acc8e378cc496c491a0124caccf6b771f9b8a Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Thu, 26 Sep 2024 09:09:38 +0200
Subject: [PATCH] fix(steering-odometry): convert twist to steering angle
 (#1288) (#1296)

(cherry picked from commit 50036e109d6a03e41bb9b43a4f67411887b48640)

Co-authored-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
---
 steering_controllers_library/src/steering_odometry.cpp | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 3141eefc34..f0d4a9e926 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -209,12 +209,9 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_
 
 double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz)
 {
-  if (fabs(v_bx) < std::numeric_limits<float>::epsilon())
-  {
-    // avoid division by zero
-    return 0.;
-  }
-  return std::atan(omega_bz * wheelbase_ / v_bx);
+  // phi can be nan if both v_bx and omega_bz are zero
+  const auto phi = std::atan(omega_bz * wheelbase_ / v_bx);
+  return std::isfinite(phi) ? phi : 0.0;
 }
 
 std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(