Don't compare double with == -1

ros-controls · Jun 18, 2024 · 88666eb · 88666eb
1 parent 597a2b8
commit 88666eb
Showing 1 changed file with 9 additions and 6 deletions.
diff --git 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;
     }