CostmapFilter::worldToMask() won't process some points

[AlexeyMerzlyakov]

CostmapFilter::worldToMask() does not process correctly some points from world that belonging to mask.
For example, consider an OccupancyGrid mask of size 3x3 with origin in (3.0, 3.0) world point.
Taking resolution = 1.0 m and this mask should cover 9 square meters in the world, starting from (3.0, 3.0) world point up to (6.0, 6.0) world point as shown in the illustration below. The world point (5.9, 5.9) in this case should belong to the mask. However, in current implementation, CostmapFilter::worldToMask(mask, 5.9, 5.9) will return false value indicating that the underlying world point does not belong to the filter mask, which is incorrect.

This appears because of CostmapFilter::worldToMask() incorrectly uses std::round instead of type casting with decimal fraction drop. This leads to CostmapFilter::worldToMask() converts to mask only (3.0, 3.0) ... (5.49999, 5.49999) world coordinates, depicted in blue at the illustration below:

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • ROS2 rolling built from sources
• Version or commit hash:
  • 4aaefad
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

• Create OccupancyGrid filter mask with 3x3 size, (3.0, 3.0) origin and resolution = 1.0
• call CostmapFilter::worldToMask(filter_mask, 5.9, 5.9, mx, my) with (5.9, 5.9) world coordinate

Expected behavior

• CostmapFilter::worldToMask() should return true and (mx, my) = (2, 2)

Actual behavior

• CostmapFilter::worldToMask() returns false

Additional information

The problem is being fixed by the following change:

--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -197,8 +197,8 @@ bool CostmapFilter::worldToMask(
     return false;
   }
 
-  mx = std::round((wx - origin_x) / resolution);
-  my = std::round((wy - origin_y) / resolution);
+  mx = static_cast<unsigned int>((wx - origin_x) / resolution);
+  my = static_cast<unsigned int>((wy - origin_y) / resolution);
   if (mx >= size_x || my >= size_y) {
     return false;
   }

Which is also consistent with current Costmap2D::worldToMap and NavfnPlanner::worldToMap and implementations.