Fix mask coordinates calculation in worldToMask (ros-navigation#3418)

Author: AlexeyMerzlyakov

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -187,18 +187,18 @@ bool CostmapFilter::worldToMask(
   nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
   double wx, double wy, unsigned int & mx, unsigned int & my) const
 {
-  double origin_x = filter_mask->info.origin.position.x;
-  double origin_y = filter_mask->info.origin.position.y;
-  double resolution = filter_mask->info.resolution;
-  unsigned int size_x = filter_mask->info.width;
-  unsigned int size_y = filter_mask->info.height;
+  const double origin_x = filter_mask->info.origin.position.x;
+  const double origin_y = filter_mask->info.origin.position.y;
+  const double resolution = filter_mask->info.resolution;
+  const unsigned int size_x = filter_mask->info.width;
+  const unsigned int size_y = filter_mask->info.height;
 
   if (wx < origin_x || wy < origin_y) {
     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;
   }

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -18,6 +18,11 @@ target_link_libraries(declare_parameter_test
   nav2_costmap_2d_core
 )
 
+ament_add_gtest(costmap_filter_test costmap_filter_test.cpp)
+target_link_libraries(costmap_filter_test
+  nav2_costmap_2d_core
+)
+
 ament_add_gtest(keepout_filter_test keepout_filter_test.cpp)
 target_link_libraries(keepout_filter_test
   nav2_costmap_2d_core

nav2_costmap_2d/test/unit/costmap_filter_test.cpp

@@ -0,0 +1,105 @@
+// Copyright (c) 2023 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/occ_grid_values.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
+
+class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
+{
+public:
+  CostmapFilterWrapper() {}
+
+  bool worldToMask(
+    nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
+    double wx, double wy, unsigned int & mx, unsigned int & my) const
+  {
+    return nav2_costmap_2d::CostmapFilter::worldToMask(filter_mask, wx, wy, mx, my);
+  }
+
+  // API coverage
+  void initializeFilter(const std::string &) {}
+  void process(
+    nav2_costmap_2d::Costmap2D &, int, int, int, int, const geometry_msgs::msg::Pose2D &)
+  {}
+  void resetFilter() {}
+};
+
+TEST(CostmapFilter, testWorldToMask)
+{
+  // Create occupancy grid for test as follows:
+  //
+  //   ^
+  //   |        (6,6)
+  //   |    *-----*
+  //   |    |/////| <- mask
+  //   |    |/////|
+  //   |    *-----*
+  //   |  (3,3)
+  //   *---------------->
+  // (0,0)
+
+  const unsigned int width = 3;
+  const unsigned int height = 3;
+
+  auto mask = std::make_shared<nav_msgs::msg::OccupancyGrid>();
+  mask->header.frame_id = "map";
+  mask->info.resolution = 1.0;
+  mask->info.width = width;
+  mask->info.height = height;
+  mask->info.origin.position.x = 3.0;
+  mask->info.origin.position.y = 3.0;
+
+  mask->data.resize(width * height, nav2_util::OCC_GRID_OCCUPIED);
+
+  CostmapFilterWrapper cf;
+  unsigned int mx, my;
+  // Point inside mask
+  ASSERT_TRUE(cf.worldToMask(mask, 4.0, 5.0, mx, my));
+  ASSERT_EQ(mx, 1);
+  ASSERT_EQ(my, 2);
+  // Corner cases
+  ASSERT_TRUE(cf.worldToMask(mask, 3.0, 3.0, mx, my));
+  ASSERT_EQ(mx, 0);
+  ASSERT_EQ(my, 0);
+  ASSERT_TRUE(cf.worldToMask(mask, 5.9, 5.9, mx, my));
+  ASSERT_EQ(mx, 2);
+  ASSERT_EQ(my, 2);
+  // Point outside mask
+  ASSERT_FALSE(cf.worldToMask(mask, 2.9, 2.9, mx, my));
+  ASSERT_FALSE(cf.worldToMask(mask, 6.0, 6.0, mx, my));
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize the system
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  // Actual testing
+  bool test_result = RUN_ALL_TESTS();
+
+  // Shutdown
+  rclcpp::shutdown();
+
+  return test_result;
+}