From 1504bc5835f38edc1a47e5adf33b8f08e4afe2cb Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 19 Jun 2024 20:23:33 +0000
Subject: [PATCH] Add helper function

---
 .../steering_controllers_library/steering_odometry.hpp      | 4 ++++
 steering_controllers_library/src/steering_odometry.cpp      | 6 +++---
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
index 8b37df384e..f668bac979 100644
--- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
+++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -18,6 +18,7 @@
 #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
 #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
 
+#include <cmath>
 #include <tuple>
 #include <vector>
 
@@ -36,6 +37,9 @@ namespace steering_odometry
 const unsigned int BICYCLE_CONFIG = 0;
 const unsigned int TRICYCLE_CONFIG = 1;
 const unsigned int ACKERMANN_CONFIG = 2;
+
+inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; }
+
 /**
  * \brief The Odometry class handles odometry readings
  * (2D pose and velocity with related timestamp)
diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
index 05850f7cf3..f8b42771e2 100644
--- a/steering_controllers_library/src/steering_odometry.cpp
+++ b/steering_controllers_library/src/steering_odometry.cpp
@@ -250,7 +250,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
     std::vector<double> traction_commands;
     std::vector<double> steering_commands;
     // double-traction axle
-    if (fabs(phi_IK) < 1e-6)
+    if (is_close_to_zero(phi_IK))
     {
       // avoid division by zero
       traction_commands = {Ws, Ws};
@@ -270,7 +270,7 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
   {
     std::vector<double> traction_commands;
     std::vector<double> steering_commands;
-    if (fabs(phi_IK) < 1e-6)
+    if (is_close_to_zero(phi_IK))
     {
       // avoid division by zero
       traction_commands = {Ws, Ws};
@@ -327,7 +327,7 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co
   const double delta_x_b = v_bx * dt;
   const double delta_theta = omega_bz * dt;
 
-  if (fabs(delta_theta) < 1e-6)
+  if (is_close_to_zero(delta_theta))
   {
     /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero):
     integrate_runge_kutta_2(v_bx, omega_bz, dt);