Merge pull request #69 from logivations/update_with_min

add function updateWithMin
logivations · Apr 17, 2024 · eac506a · eac506a
2 parents cd0c681 + 10e011c
commit eac506a
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 0 deletions.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -128,6 +128,20 @@ class CostmapLayer : public Layer, public Costmap2D
     nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
     int max_j);
 
+
+  /*
+   * Updates the master_grid within the specified
+   * bounding box using this layer's values.
+   *
+   * Sets the new value to the minimum of the master_grid's value
+   * and this layer's value. If the master value is NO_INFORMATION,
+   * it is overwritten. If the layer's value is NO_INFORMATION,
+   * the master value does not change.
+   */
+  void updateWithMin(
+    nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
+    int max_j);
+
   /*
    * Updates the master_grid within the specified
    * bounding box using this layer's values.

diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -548,6 +548,9 @@ ObstacleLayer::updateCosts(
     case 1:  // Maximum
       updateWithMax(master_grid, min_i, min_j, max_i, max_j);
       break;
+    case 2:  // Minimum
+      updateWithMin(master_grid, min_i, min_j, max_i, max_j);
+      break;
     default:  // Nothing
       break;
   }

diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -135,6 +135,34 @@ void CostmapLayer::updateWithMax(
     }
   }
 }
+void CostmapLayer::updateWithMin(
+  nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
+  int max_i,
+  int max_j)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  unsigned char * master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++) {
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++) {
+      if (costmap_[it] == NO_INFORMATION) {
+        it++;
+        continue;
+      }
+
+      unsigned char old_cost = master_array[it];
+      if (old_cost == NO_INFORMATION || old_cost > costmap_[it]) {
+        master_array[it] = costmap_[it];
+      }
+      it++;
+    }
+  }
+}
 
 void CostmapLayer::updateWithTrueOverwrite(
   nav2_costmap_2d::Costmap2D & master_grid, int min_i,