[static_layer] limit comparison precision (#5405)

* [DEX] limit comparison precision

Signed-off-by: Guillaume Doisy <[email protected]>

* EPSILON 1e-5

Signed-off-by: Guillaume Doisy <[email protected]>

---------

Signed-off-by: Guillaume Doisy <[email protected]>
Co-authored-by: Guillaume Doisy <[email protected]>

Author: 2 people

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -154,6 +154,15 @@ class StaticLayer : public CostmapLayer
    */
   unsigned char interpretValue(unsigned char value);
 
+  /**
+   * @brief Check if two double values are equal within a given epsilon
+   * @param a First double value
+   * @param b Second double value
+   * @param epsilon The tolerance for equality check
+   * @return True if the values are equal within the tolerance, false otherwise
+   */
+  bool isEqual(double a, double b, double epsilon);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message

nav2_costmap_2d/plugins/static_layer.cpp

@@ -47,6 +47,8 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_util/validate_messages.hpp"
 
+#define EPSILON 1e-5
+
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
 using nav2_costmap_2d::NO_INFORMATION;
@@ -196,9 +198,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
   Costmap2D * master = layered_costmap_->getCostmap();
   if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
     master->getSizeInCellsY() != size_y ||
-    master->getResolution() != new_map.info.resolution ||
-    master->getOriginX() != new_map.info.origin.position.x ||
-    master->getOriginY() != new_map.info.origin.position.y ||
+    !isEqual(master->getResolution(), new_map.info.resolution, EPSILON) ||
+    !isEqual(master->getOriginX(), new_map.info.origin.position.x, EPSILON) ||
+    !isEqual(master->getOriginY(), new_map.info.origin.position.y, EPSILON) ||
     !layered_costmap_->isSizeLocked()))
   {
     // Update the size of the layered costmap (and all layers, including this one)
@@ -212,9 +214,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
       new_map.info.origin.position.y,
       true);
   } else if (size_x_ != size_x || size_y_ != size_y ||  // NOLINT
-    resolution_ != new_map.info.resolution ||
-    origin_x_ != new_map.info.origin.position.x ||
-    origin_y_ != new_map.info.origin.position.y)
+    !isEqual(resolution_, new_map.info.resolution, EPSILON) ||
+    !isEqual(origin_x_, new_map.info.origin.position.x, EPSILON) ||
+    !isEqual(origin_y_, new_map.info.origin.position.y, EPSILON))
   {
     // only update the size of the costmap stored locally in this layer
     RCLCPP_INFO(
@@ -467,6 +469,18 @@ StaticLayer::updateCosts(
   current_ = true;
 }
 
+/**
+  * @brief Check if two floating point numbers are equal within a given epsilon
+  * @param a First number
+  * @param b Second number
+  * @param epsilon Tolerance for equality check
+  * @return True if numbers are equal within the tolerance, false otherwise
+  */
+bool StaticLayer::isEqual(double a, double b, double epsilon)
+{
+  return std::abs(a - b) < epsilon;
+}
+
 /**
   * @brief Callback executed when a parameter change is detected
   * @param event ParameterEvent message