Add helper function

ros-controls · Jun 19, 2024 · 1504bc5 · 1504bc5
1 parent 37b8f0a
commit 1504bc5
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 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
@@ -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
@@ -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);