fix: Set disabled plugins to current even with no bounds to update

nav2_costmap_2d/src/layered_costmap.cpp

@@ -213,54 +213,52 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
     rclcpp::get_logger(
       "nav2_costmap_2d"), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);
 
-  if (xn < x0 || yn < y0) {
-    return;
-  }
-
-  if (filters_.size() == 0) {
-    // If there are no filters enabled just update costmap sequentially by each plugin
-    combined_costmap_.resetMap(x0, y0, xn, yn);
-    for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
-      plugin != plugins_.end(); ++plugin)
-    {
-      (*plugin)->updateCosts(combined_costmap_, x0, y0, xn, yn);
-    }
-  } else {
-    // Costmap Filters enabled
-    // 1. Update costmap by plugins
-    primary_costmap_.resetMap(x0, y0, xn, yn);
-    for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
-      plugin != plugins_.end(); ++plugin)
-    {
-      (*plugin)->updateCosts(primary_costmap_, x0, y0, xn, yn);
+  if (xn >= x0 && yn >= y0) {
+    if (filters_.size() == 0) {
+      // If there are no filters enabled just update costmap sequentially by each plugin
+      combined_costmap_.resetMap(x0, y0, xn, yn);
+      for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+        plugin != plugins_.end(); ++plugin)
+      {
+        (*plugin)->updateCosts(combined_costmap_, x0, y0, xn, yn);
+      }
+    } else {
+      // Costmap Filters enabled
+      // 1. Update costmap by plugins
+      primary_costmap_.resetMap(x0, y0, xn, yn);
+      for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
+        plugin != plugins_.end(); ++plugin)
+      {
+        (*plugin)->updateCosts(primary_costmap_, x0, y0, xn, yn);
+      }
+
+      // 2. Copy processed costmap window to a final costmap.
+      // primary_costmap_ remain to be untouched for further usage by plugins.
+      if (!combined_costmap_.copyWindow(primary_costmap_, x0, y0, xn, yn, x0, y0)) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("nav2_costmap_2d"),
+          "Can not copy costmap (%i,%i)..(%i,%i) window",
+          x0, y0, xn, yn);
+        throw std::runtime_error{"Can not copy costmap"};
+      }
+
+      // 3. Apply filters over the plugins in order to make filters' work
+      // not being considered by plugins on next updateMap() calls
+      for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
+        filter != filters_.end(); ++filter)
+      {
+        (*filter)->updateCosts(combined_costmap_, x0, y0, xn, yn);
+      }
     }
 
-    // 2. Copy processed costmap window to a final costmap.
-    // primary_costmap_ remain to be untouched for further usage by plugins.
-    if (!combined_costmap_.copyWindow(primary_costmap_, x0, y0, xn, yn, x0, y0)) {
-      RCLCPP_ERROR(
-        rclcpp::get_logger("nav2_costmap_2d"),
-        "Can not copy costmap (%i,%i)..(%i,%i) window",
-        x0, y0, xn, yn);
-      throw std::runtime_error{"Can not copy costmap"};
-    }
+    bx0_ = x0;
+    bxn_ = xn;
+    by0_ = y0;
+    byn_ = yn;
 
-    // 3. Apply filters over the plugins in order to make filters' work
-    // not being considered by plugins on next updateMap() calls
-    for (vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin();
-      filter != filters_.end(); ++filter)
-    {
-      (*filter)->updateCosts(combined_costmap_, x0, y0, xn, yn);
-    }
+    initialized_ = true;
   }
 
-  bx0_ = x0;
-  bxn_ = xn;
-  by0_ = y0;
-  byn_ = yn;
-
-  initialized_ = true;
-
   // Set current_ = true for all disabled plugins
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
     plugin != plugins_.end(); ++plugin)