From 88666ebd151badb1a197e1e994a22d7d1a6b7886 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Tue, 18 Jun 2024 09:45:17 +0000
Subject: [PATCH] Don't compare double with == -1

---
 .../joint_trajectory_controller/tolerances.hpp    | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
index acdd2da86f..03d6511598 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
@@ -30,6 +30,7 @@
 #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
 
+#include <limits>
 #include <vector>
 
 #include "control_msgs/action/follow_joint_trajectory.hpp"
@@ -148,6 +149,8 @@ SegmentTolerances get_segment_tolerances(
   // * -1 - The tolerance is "erased".
   //        If there was a default, the joint will be  allowed to move without restriction.
   constexpr double ERASE_VALUE = -1.0;
+  auto is_erase_value = [](double value)
+  { return fabs(value - ERASE_VALUE) < std::numeric_limits<float>::epsilon(); };
 
   // State and goal state tolerances
   for (auto joint_tol : goal.path_tolerance)
@@ -167,7 +170,7 @@ SegmentTolerances get_segment_tolerances(
     {
       active_tolerances.state_tolerance[i].position = joint_tol.position;
     }
-    else if (joint_tol.position == ERASE_VALUE)
+    else if (is_erase_value(joint_tol.position))
     {
       active_tolerances.state_tolerance[i].position = 0.0;
     }
@@ -176,7 +179,7 @@ SegmentTolerances get_segment_tolerances(
     {
       active_tolerances.state_tolerance[i].velocity = joint_tol.velocity;
     }
-    else if (joint_tol.velocity == ERASE_VALUE)
+    else if (is_erase_value(joint_tol.velocity))
     {
       active_tolerances.state_tolerance[i].velocity = 0.0;
     }
@@ -185,7 +188,7 @@ SegmentTolerances get_segment_tolerances(
     {
       active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration;
     }
-    else if (joint_tol.acceleration == ERASE_VALUE)
+    else if (is_erase_value(joint_tol.acceleration))
     {
       active_tolerances.state_tolerance[i].acceleration = 0.0;
     }
@@ -217,7 +220,7 @@ SegmentTolerances get_segment_tolerances(
     {
       active_tolerances.goal_state_tolerance[i].position = goal_tol.position;
     }
-    else if (goal_tol.position == ERASE_VALUE)
+    else if (is_erase_value(goal_tol.position))
     {
       active_tolerances.goal_state_tolerance[i].position = 0.0;
     }
@@ -226,7 +229,7 @@ SegmentTolerances get_segment_tolerances(
     {
       active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity;
     }
-    else if (goal_tol.velocity == ERASE_VALUE)
+    else if (is_erase_value(goal_tol.velocity))
     {
       active_tolerances.goal_state_tolerance[i].velocity = 0.0;
     }
@@ -235,7 +238,7 @@ SegmentTolerances get_segment_tolerances(
     {
       active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration;
     }
-    else if (goal_tol.acceleration == ERASE_VALUE)
+    else if (is_erase_value(goal_tol.acceleration))
     {
       active_tolerances.goal_state_tolerance[i].acceleration = 0.0;
     }