diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp index 15285df5fe2..3e3ba21703d 100644 --- 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 index 2a5474c256d..89c93f13a1f 100644 --- 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 index ad488c26185..6137df58134 100644 --- 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,