Fix floating point comparison in JTC (backport ros-controls#879)

* Fix floating point comparison in JTC * Fix format --------- Co-authored-by: Christoph Fröhlich <[email protected]>
christophfroehlich · Dec 4, 2023 · 493990d · 493990d
1 parent 416d767
commit 493990d
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -1467,10 +1467,11 @@ bool JointTrajectoryController::validate_trajectory_msg(
   {
     for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i)
     {
-      if (trajectory.points.back().velocities.at(i) != 0.)
+      if (fabs(trajectory.points.back().velocities.at(i)) > std::numeric_limits<float>::epsilon())
       {
         RCLCPP_ERROR(
-          get_node()->get_logger(), "Velocity of last trajectory point of joint %s is not zero: %f",
+          get_node()->get_logger(),
+          "Velocity of last trajectory point of joint %s is not zero: %.15f",
           trajectory.joint_names.at(i).c_str(), trajectory.points.back().velocities.at(i));
         return false;
       }