Clear costmap if reset distance exceeds costmap bounds. (ros-navigation#5010)

* Migrate costmap bound check to clearArea for STVL override

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added unbounded map to world function.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

* Added test for mapToWorldNoBounds

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

---------

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>
Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

Author: leander-dsouza

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -173,6 +173,15 @@ class Costmap2D
    */
   void mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const;
 
+  /**
+  * @brief  Convert from map coordinates to world coordinates with no bounds checking
+  * @param  wx The x world coordinate
+  * @param  wy The y world coordinate
+  * @param  mx Will be set to the associated map x coordinate
+  * @param  my Will be set to the associated map y coordinate
+  */
+  void mapToWorldNoBounds(int mx, int my, double & wx, double & wy) const;
+
   /**
    * @brief  Convert from world coordinates to map coordinates
    * @param  wx The x world coordinate

nav2_costmap_2d/src/clear_costmap_service.cpp

@@ -135,8 +135,8 @@ void ClearCostmapService::clearLayerRegion(
   double end_point_y = start_point_y + reset_distance;
 
   int start_x, start_y, end_x, end_y;
-  costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y);
-  costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y);
+  costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+  costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
 
   costmap->clearArea(start_x, start_y, end_x, end_y, invert);
 

nav2_costmap_2d/src/costmap_2d.cpp

@@ -282,6 +282,12 @@ void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double
   wy = origin_y_ + (my + 0.5) * resolution_;
 }
 
+void Costmap2D::mapToWorldNoBounds(int mx, int my, double & wx, double & wy) const
+{
+  wx = origin_x_ + (mx + 0.5) * resolution_;
+  wy = origin_y_ + (my + 0.5) * resolution_;
+}
+
 bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const
 {
   if (wx < origin_x_ || wy < origin_y_) {

nav2_costmap_2d/src/costmap_layer.cpp

@@ -66,10 +66,19 @@ void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, boo
 {
   current_ = false;
   unsigned char * grid = getCharMap();
-  for (int x = 0; x < static_cast<int>(getSizeInCellsX()); x++) {
+
+  int size_x = getSizeInCellsX();
+  int size_y = getSizeInCellsY();
+
+  start_x = std::clamp(start_x, 0, size_x);
+  start_y = std::clamp(start_y, 0, size_y);
+  end_x = std::clamp(end_x, 0, size_x);
+  end_y = std::clamp(end_y, 0, size_y);
+
+  for (int x = 0; x < size_x; x++) {
     bool xrange = x > start_x && x < end_x;
 
-    for (int y = 0; y < static_cast<int>(getSizeInCellsY()); y++) {
+    for (int y = 0; y < size_y; y++) {
       if ((xrange && y > start_y && y < end_y) == invert) {
         continue;
       }

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -62,3 +62,8 @@ ament_add_gtest(lifecycle_test lifecycle_test.cpp)
 target_link_libraries(lifecycle_test
   nav2_costmap_2d_core
 )
+
+ament_add_gtest(coordinate_transform_test coordinate_transform_test.cpp)
+target_link_libraries(coordinate_transform_test
+  nav2_costmap_2d_core
+)

nav2_costmap_2d/test/unit/coordinate_transform_test.cpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2025 Open Navigation LLC
+//
+// 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 "rclcpp/rclcpp.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+
+
+/**
+ * Test for mapToWorldNoBounds
+ */
+
+TEST(mapToWorldNoBounds, MapToWorldNoBoundsNegativeMapCoords)
+{
+  double wx, wy;
+
+  std::unique_ptr<nav2_costmap_2d::Costmap2D> map;
+
+  map = std::make_unique<nav2_costmap_2d::Costmap2D>(10, 10, 1.0, 0.0, 0.0);
+  map->mapToWorldNoBounds(-1, -1, wx, wy);
+  EXPECT_DOUBLE_EQ(wx, -0.5);
+  EXPECT_DOUBLE_EQ(wy, -0.5);
+
+  map = std::make_unique<nav2_costmap_2d::Costmap2D>(10, 10, 1.0, 1.0, 2.0);
+  map->mapToWorldNoBounds(-5, -5, wx, wy);
+  EXPECT_DOUBLE_EQ(wx, -3.5);
+  EXPECT_DOUBLE_EQ(wy, -2.5);
+
+  map = std::make_unique<nav2_costmap_2d::Costmap2D>(10, 10, 2.0, 3.0, 4.0);
+  map->mapToWorldNoBounds(-10, -10, wx, wy);
+  EXPECT_DOUBLE_EQ(wx, -16.0);
+  EXPECT_DOUBLE_EQ(wy, -15.0);
+}
+
+
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  rclcpp::init(0, nullptr);
+
+  int result = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+
+  return result;
+}