From b50a6bfb47b55acc078c7c1a4db8e324a81746ad Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Fri, 9 Jun 2023 08:37:34 +0200
Subject: [PATCH 01/11] Add updateWithMaxWithoutUnknownOverride

---
 .../include/nav2_costmap_2d/costmap_layer.hpp | 13 +++++++++
 nav2_costmap_2d/plugins/obstacle_layer.cpp    |  2 ++
 nav2_costmap_2d/src/costmap_layer.cpp         | 29 +++++++++++++++++++
 3 files changed, 44 insertions(+)

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..d8104a694b9 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,19 @@ 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 maximum of the master_grid's value
+   * and this layer's value. If the master value is NO_INFORMATION,
+   * it is NOT overwritten. If the layer's value is NO_INFORMATION,
+   * the master value does not change.
+   */
+  void updateWithMaxWithoutUnknownOverride(
+    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 653c0885cae..4f4cbeb51d9 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -546,6 +546,8 @@ ObstacleLayer::updateCosts(
     case 1:  // Maximum
       updateWithMax(master_grid, min_i, min_j, max_i, max_j);
       break;
+    case 2: // Maximum without overwrite if unknown
+      updateWithMaxWithoutUnknownOverride(master_grid, min_i, min_j, max_i, max_j);
     default:  // Nothing
       break;
   }
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index ad488c26185..39b2cd26cc9 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -136,6 +136,35 @@ void CostmapLayer::updateWithMax(
   }
 }
 
+void CostmapLayer::updateWithMaxWithoutUnknownOverride(
+  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 < costmap_[it]) {
+        master_array[it] = costmap_[it];
+      }
+      it++;
+    }
+  }
+}
+
 void CostmapLayer::updateWithTrueOverwrite(
   nav2_costmap_2d::Costmap2D & master_grid, int min_i,
   int min_j,

From c6e7b42e5d37bf6eaff31308bb4fe4ebd378f6a1 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Fri, 9 Jun 2023 09:36:54 +0200
Subject: [PATCH 02/11] Add missing break to switch case

---
 nav2_costmap_2d/plugins/obstacle_layer.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 4f4cbeb51d9..8013181a7f2 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -548,6 +548,7 @@ ObstacleLayer::updateCosts(
       break;
     case 2: // Maximum without overwrite if unknown
       updateWithMaxWithoutUnknownOverride(master_grid, min_i, min_j, max_i, max_j);
+      break;
     default:  // Nothing
       break;
   }

From 3ed252b65e0704c9652f89b2ce71bc2efd420e87 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Fri, 9 Jun 2023 16:05:45 +0200
Subject: [PATCH 03/11] Add additional NO_INFORMATION check to make more robust

---
 nav2_costmap_2d/src/costmap_layer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index 39b2cd26cc9..d23c51cdc95 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -157,7 +157,7 @@ void CostmapLayer::updateWithMaxWithoutUnknownOverride(
       }
 
       unsigned char old_cost = master_array[it];
-      if (old_cost < costmap_[it]) {
+      if (old_cost != NO_INFORMATION && old_cost < costmap_[it]) {
         master_array[it] = costmap_[it];
       }
       it++;

From 8dd10bfb1c0ebf2790fc07bd5e7e58a47eae8684 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Mon, 12 Jun 2023 11:38:35 +0200
Subject: [PATCH 04/11] Add CombinationMethod enum with
 combination_method_from_int

---
 .../nav2_costmap_2d/obstacle_layer.hpp        | 33 ++++++++++++++++++-
 nav2_costmap_2d/plugins/obstacle_layer.cpp    | 33 ++++++++++++++++---
 nav2_costmap_2d/plugins/voxel_layer.cpp       |  9 +++--
 3 files changed, 67 insertions(+), 8 deletions(-)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 754b434930c..30c0bf6bbe7 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -61,6 +61,37 @@
 namespace nav2_costmap_2d
 {
 
+/**
+ * @enum nav2_costmap_2d::CombinationMethod
+ * @brief Describes the method used to add data to master costmap, default to maximum.
+ */
+enum class CombinationMethod : int
+{
+  /**
+   * Overwrite means every valid value from this layer
+   * is written into the master grid (does not copy NO_INFORMATION)
+   */
+  Overwrite = 0,
+  /**
+   * Sets the new value to the maximum 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
+   */
+  Max = 1,
+  /**
+   * Sets the new value to the maximum of the master_grid's value
+   * and this layer's value. If the master value is NO_INFORMATION,
+   * it is NOT overwritten. If the layer's value is NO_INFORMATION,
+   * the master value does not change.
+   */
+  MaxWithoutUnknownOverride = 2
+};
+  /**
+   * @brief Converts an integer to a CombinationMethod enum and logs on failure
+   */
+CombinationMethod combination_method_from_int (const int value, const std::string function_name);
+
 /**
  * @class ObstacleLayer
  * @brief Takes in laser and pointcloud data to populate into 2D costmap
@@ -252,7 +283,7 @@ class ObstacleLayer : public CostmapLayer
 
   bool rolling_window_;
   bool was_reset_;
-  int combination_method_;
+  nav2_costmap_2d::CombinationMethod combination_method_;
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 8013181a7f2..e64d3320cf2 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -92,11 +92,15 @@ void ObstacleLayer::onInitialize()
   node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
   node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
   node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
-  node->get_parameter(name_ + "." + "combination_method", combination_method_);
   node->get_parameter("track_unknown_space", track_unknown_space);
   node->get_parameter("transform_tolerance", transform_tolerance);
   node->get_parameter(name_ + "." + "observation_sources", topics_string);
 
+  int combination_method_param{};
+  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+  combination_method_ = combination_method_from_int(combination_method_param,
+    "ObstacleLayer::onInitialize()");
+
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(
       &ObstacleLayer::dynamicParametersCallback,
@@ -311,7 +315,8 @@ ObstacleLayer::dynamicParametersCallback(
       }
     } else if (param_type == ParameterType::PARAMETER_INTEGER) {
       if (param_name == name_ + "." + "combination_method") {
-        combination_method_ = parameter.as_int();
+        combination_method_ = combination_method_from_int(parameter.as_int(), "ObstacleLayer::"
+          "dynamicParametersCallback()");
       }
     }
   }
@@ -540,13 +545,13 @@ ObstacleLayer::updateCosts(
   }
 
   switch (combination_method_) {
-    case 0:  // Overwrite
+    case CombinationMethod::Overwrite:
       updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
       break;
-    case 1:  // Maximum
+    case CombinationMethod::Max:
       updateWithMax(master_grid, min_i, min_j, max_i, max_j);
       break;
-    case 2: // Maximum without overwrite if unknown
+    case CombinationMethod::MaxWithoutUnknownOverride:
       updateWithMaxWithoutUnknownOverride(master_grid, min_i, min_j, max_i, max_j);
       break;
     default:  // Nothing
@@ -763,4 +768,22 @@ ObstacleLayer::resetBuffersLastUpdated()
   }
 }
 
+CombinationMethod combination_method_from_int (const int value, const std::string function_name){
+    switch(value){
+    case 0:
+      return CombinationMethod::Overwrite;
+    case 1:
+      return CombinationMethod::Max;
+    case 2:
+      return CombinationMethod::MaxWithoutUnknownOverride;
+    default:
+      RCLCPP_WARN(
+        rclcpp::get_logger("nav2_costmap_2d"),
+        (function_name + ": param combination_method: %i. Possible values are  0 (Overwrite) or 1 (Maximum) or "
+        "2 (Maximum without overwriting the master's NO_INFORMATION values)."
+        "The default value 1 will be used").c_str(), value);
+      return CombinationMethod::Max;
+  }
+}
+
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 21d6e669011..87a29e34532 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -86,9 +86,13 @@ void VoxelLayer::onInitialize()
   node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
   node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
   node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
-  node->get_parameter(name_ + "." + "combination_method", combination_method_);
   node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
 
+  int combination_method_param{};
+  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+  combination_method_ = combination_method_from_int(combination_method_param,
+    "VoxelLayer::onInitialize()");
+
   auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
 
   if (publish_voxel_) {
@@ -522,7 +526,8 @@ VoxelLayer::dynamicParametersCallback(
       } else if (param_name == name_ + "." + "mark_threshold") {
         mark_threshold_ = parameter.as_int();
       } else if (param_name == name_ + "." + "combination_method") {
-        combination_method_ = parameter.as_int();
+        combination_method_ = combination_method_from_int(parameter.as_int(), "VoxelLayer::"
+          "dynamicParametersCallback()");
       }
     }
   }

From 818b1057aedff66c71af67e197c02168cb8fa684 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Mon, 12 Jun 2023 11:43:52 +0200
Subject: [PATCH 05/11] Rename override to overwrite

---
 nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp  | 2 +-
 nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp | 2 +-
 nav2_costmap_2d/plugins/obstacle_layer.cpp                 | 6 +++---
 nav2_costmap_2d/src/costmap_layer.cpp                      | 2 +-
 4 files changed, 6 insertions(+), 6 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
index d8104a694b9..c197689ef79 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -137,7 +137,7 @@ class CostmapLayer : public Layer, public Costmap2D
    * it is NOT overwritten. If the layer's value is NO_INFORMATION,
    * the master value does not change.
    */
-  void updateWithMaxWithoutUnknownOverride(
+  void updateWithMaxWithoutUnknownOverwrite(
     nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
     int max_j);
 
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 30c0bf6bbe7..9a0bc1b4a8f 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -85,7 +85,7 @@ enum class CombinationMethod : int
    * it is NOT overwritten. If the layer's value is NO_INFORMATION,
    * the master value does not change.
    */
-  MaxWithoutUnknownOverride = 2
+  MaxWithoutUnknownOverwrite = 2
 };
   /**
    * @brief Converts an integer to a CombinationMethod enum and logs on failure
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index e64d3320cf2..c388e195821 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -551,8 +551,8 @@ ObstacleLayer::updateCosts(
     case CombinationMethod::Max:
       updateWithMax(master_grid, min_i, min_j, max_i, max_j);
       break;
-    case CombinationMethod::MaxWithoutUnknownOverride:
-      updateWithMaxWithoutUnknownOverride(master_grid, min_i, min_j, max_i, max_j);
+    case CombinationMethod::MaxWithoutUnknownOverwrite:
+      updateWithMaxWithoutUnknownOverwrite(master_grid, min_i, min_j, max_i, max_j);
       break;
     default:  // Nothing
       break;
@@ -775,7 +775,7 @@ CombinationMethod combination_method_from_int (const int value, const std::strin
     case 1:
       return CombinationMethod::Max;
     case 2:
-      return CombinationMethod::MaxWithoutUnknownOverride;
+      return CombinationMethod::MaxWithoutUnknownOverwrite;
     default:
       RCLCPP_WARN(
         rclcpp::get_logger("nav2_costmap_2d"),
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index d23c51cdc95..879ea106308 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -136,7 +136,7 @@ void CostmapLayer::updateWithMax(
   }
 }
 
-void CostmapLayer::updateWithMaxWithoutUnknownOverride(
+void CostmapLayer::updateWithMaxWithoutUnknownOverwrite(
   nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
   int max_i,
   int max_j)

From 4144a0b07d1dee072fd53bfb14af1105ad811a0a Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Mon, 12 Jun 2023 11:49:24 +0200
Subject: [PATCH 06/11] Update docs of combination_method_from_int

---
 nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 9a0bc1b4a8f..6792febe4f1 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -89,6 +89,8 @@ enum class CombinationMethod : int
 };
   /**
    * @brief Converts an integer to a CombinationMethod enum and logs on failure
+   * @param value The integer to convert
+   * @param function_name The name of the function calling this conversion (for logging)
    */
 CombinationMethod combination_method_from_int (const int value, const std::string function_name);
 

From cdabf968761fed68215d06512af48bd681bb53c0 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Tue, 13 Jun 2023 13:13:01 +0200
Subject: [PATCH 07/11] Move definitions to costmap_layer and remove
 function_name param

---
 .../include/nav2_costmap_2d/cost_values.hpp   | 28 ++++++++++++++++
 .../include/nav2_costmap_2d/costmap_layer.hpp |  7 ++++
 .../nav2_costmap_2d/obstacle_layer.hpp        | 33 -------------------
 nav2_costmap_2d/plugins/obstacle_layer.cpp    | 24 ++------------
 nav2_costmap_2d/plugins/voxel_layer.cpp       |  6 ++--
 nav2_costmap_2d/src/costmap_layer.cpp         | 18 ++++++++++
 6 files changed, 57 insertions(+), 59 deletions(-)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp
index dca22c9f7ff..6fbb5352440 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp
@@ -39,6 +39,34 @@
 /** Provides a mapping for often used cost values */
 namespace nav2_costmap_2d
 {
+
+/**
+ * @enum nav2_costmap_2d::CombinationMethod
+ * @brief Describes the method used to add data to master costmap, default to maximum.
+ */
+enum class CombinationMethod : int
+{
+  /**
+   * Overwrite means every valid value from this layer
+   * is written into the master grid (does not copy NO_INFORMATION)
+   */
+  Overwrite = 0,
+  /**
+   * Sets the new value to the maximum 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
+   */
+  Max = 1,
+  /**
+   * Sets the new value to the maximum of the master_grid's value
+   * and this layer's value. If the master value is NO_INFORMATION,
+   * it is NOT overwritten. If the layer's value is NO_INFORMATION,
+   * the master value does not change.
+   */
+  MaxWithoutUnknownOverwrite = 2
+};
+
 static constexpr unsigned char NO_INFORMATION = 255;
 static constexpr unsigned char LETHAL_OBSTACLE = 254;
 static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
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 c197689ef79..6dee8360ce5 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -185,6 +185,13 @@ class CostmapLayer : public Layer, public Costmap2D
   void useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y);
   bool has_extra_bounds_;
 
+    /**
+   * @brief Converts an integer to a CombinationMethod enum and logs on failure
+   * @param value The integer to convert
+   * @param function_name The name of the function calling this conversion (for logging)
+   */
+  CombinationMethod combination_method_from_int (const int value);
+
 private:
   double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
 };
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 6792febe4f1..4b8409f4678 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -61,39 +61,6 @@
 namespace nav2_costmap_2d
 {
 
-/**
- * @enum nav2_costmap_2d::CombinationMethod
- * @brief Describes the method used to add data to master costmap, default to maximum.
- */
-enum class CombinationMethod : int
-{
-  /**
-   * Overwrite means every valid value from this layer
-   * is written into the master grid (does not copy NO_INFORMATION)
-   */
-  Overwrite = 0,
-  /**
-   * Sets the new value to the maximum 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
-   */
-  Max = 1,
-  /**
-   * Sets the new value to the maximum of the master_grid's value
-   * and this layer's value. If the master value is NO_INFORMATION,
-   * it is NOT overwritten. If the layer's value is NO_INFORMATION,
-   * the master value does not change.
-   */
-  MaxWithoutUnknownOverwrite = 2
-};
-  /**
-   * @brief Converts an integer to a CombinationMethod enum and logs on failure
-   * @param value The integer to convert
-   * @param function_name The name of the function calling this conversion (for logging)
-   */
-CombinationMethod combination_method_from_int (const int value, const std::string function_name);
-
 /**
  * @class ObstacleLayer
  * @brief Takes in laser and pointcloud data to populate into 2D costmap
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index c388e195821..cccae4d3938 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -98,8 +98,7 @@ void ObstacleLayer::onInitialize()
 
   int combination_method_param{};
   node->get_parameter(name_ + "." + "combination_method", combination_method_param);
-  combination_method_ = combination_method_from_int(combination_method_param,
-    "ObstacleLayer::onInitialize()");
+  combination_method_ = combination_method_from_int(combination_method_param);
 
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(
@@ -315,8 +314,7 @@ ObstacleLayer::dynamicParametersCallback(
       }
     } else if (param_type == ParameterType::PARAMETER_INTEGER) {
       if (param_name == name_ + "." + "combination_method") {
-        combination_method_ = combination_method_from_int(parameter.as_int(), "ObstacleLayer::"
-          "dynamicParametersCallback()");
+        combination_method_ = combination_method_from_int(parameter.as_int());
       }
     }
   }
@@ -768,22 +766,4 @@ ObstacleLayer::resetBuffersLastUpdated()
   }
 }
 
-CombinationMethod combination_method_from_int (const int value, const std::string function_name){
-    switch(value){
-    case 0:
-      return CombinationMethod::Overwrite;
-    case 1:
-      return CombinationMethod::Max;
-    case 2:
-      return CombinationMethod::MaxWithoutUnknownOverwrite;
-    default:
-      RCLCPP_WARN(
-        rclcpp::get_logger("nav2_costmap_2d"),
-        (function_name + ": param combination_method: %i. Possible values are  0 (Overwrite) or 1 (Maximum) or "
-        "2 (Maximum without overwriting the master's NO_INFORMATION values)."
-        "The default value 1 will be used").c_str(), value);
-      return CombinationMethod::Max;
-  }
-}
-
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 87a29e34532..fc411d9f42e 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -90,8 +90,7 @@ void VoxelLayer::onInitialize()
 
   int combination_method_param{};
   node->get_parameter(name_ + "." + "combination_method", combination_method_param);
-  combination_method_ = combination_method_from_int(combination_method_param,
-    "VoxelLayer::onInitialize()");
+  combination_method_ = combination_method_from_int(combination_method_param);
 
   auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
 
@@ -526,8 +525,7 @@ VoxelLayer::dynamicParametersCallback(
       } else if (param_name == name_ + "." + "mark_threshold") {
         mark_threshold_ = parameter.as_int();
       } else if (param_name == name_ + "." + "combination_method") {
-        combination_method_ = combination_method_from_int(parameter.as_int(), "VoxelLayer::"
-          "dynamicParametersCallback()");
+        combination_method_ = combination_method_from_int(parameter.as_int());
       }
     }
   }
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index 879ea106308..a4b494ce071 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -245,4 +245,22 @@ void CostmapLayer::updateWithAddition(
     }
   }
 }
+
+CombinationMethod CostmapLayer::combination_method_from_int (const int value){
+    switch(value){
+    case 0:
+      return CombinationMethod::Overwrite;
+    case 1:
+      return CombinationMethod::Max;
+    case 2:
+      return CombinationMethod::MaxWithoutUnknownOverwrite;
+    default:
+      RCLCPP_WARN(
+        rclcpp::get_logger("nav2_costmap_2d"),
+        "Param combination_method: %i. Possible values are  0 (Overwrite) or 1 (Maximum) or "
+        "2 (Maximum without overwriting the master's NO_INFORMATION values)."
+        "The default value 1 will be used", value);
+      return CombinationMethod::Max;
+  }
+}
 }  // namespace nav2_costmap_2d

From d27a89ca020320472a96ec56fab722392a980cff Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Tue, 13 Jun 2023 13:16:17 +0200
Subject: [PATCH 08/11] Replace logger with node's logger

---
 nav2_costmap_2d/src/costmap_layer.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index a4b494ce071..bbf5a4be960 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -256,7 +256,7 @@ CombinationMethod CostmapLayer::combination_method_from_int (const int value){
       return CombinationMethod::MaxWithoutUnknownOverwrite;
     default:
       RCLCPP_WARN(
-        rclcpp::get_logger("nav2_costmap_2d"),
+        logger_,
         "Param combination_method: %i. Possible values are  0 (Overwrite) or 1 (Maximum) or "
         "2 (Maximum without overwriting the master's NO_INFORMATION values)."
         "The default value 1 will be used", value);

From d4d5ae5fd0f8483b129ba8e5bb2d89c0a383fbd4 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Tue, 13 Jun 2023 13:25:07 +0200
Subject: [PATCH 09/11] Fix linting errors

---
 .../include/nav2_costmap_2d/costmap_layer.hpp        | 12 ++++++------
 nav2_costmap_2d/src/costmap_layer.cpp                |  5 +++--
 2 files changed, 9 insertions(+), 8 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
index 6dee8360ce5..43a82e0cde1 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -185,12 +185,12 @@ class CostmapLayer : public Layer, public Costmap2D
   void useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y);
   bool has_extra_bounds_;
 
-    /**
-   * @brief Converts an integer to a CombinationMethod enum and logs on failure
-   * @param value The integer to convert
-   * @param function_name The name of the function calling this conversion (for logging)
-   */
-  CombinationMethod combination_method_from_int (const int value);
+  /**
+ * @brief Converts an integer to a CombinationMethod enum and logs on failure
+ * @param value The integer to convert
+ * @param function_name The name of the function calling this conversion (for logging)
+ */
+  CombinationMethod combination_method_from_int(const int value);
 
 private:
   double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index bbf5a4be960..8dbce5017f0 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -246,8 +246,9 @@ void CostmapLayer::updateWithAddition(
   }
 }
 
-CombinationMethod CostmapLayer::combination_method_from_int (const int value){
-    switch(value){
+CombinationMethod CostmapLayer::combination_method_from_int(const int value)
+{
+  switch (value) {
     case 0:
       return CombinationMethod::Overwrite;
     case 1:

From bd1cb79565dee202c296abe86a11e50e2608eb04 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Tue, 20 Jun 2023 14:42:43 +0200
Subject: [PATCH 10/11] Add test

---
 .../test/integration/obstacle_tests.cpp       | 47 +++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
index 707d795a34b..ca85d67eff8 100644
--- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp
+++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
@@ -554,3 +554,50 @@ TEST_F(TestNode, testDynParamsSetStatic)
   costmap->on_cleanup(rclcpp_lifecycle::State());
   costmap->on_shutdown(rclcpp_lifecycle::State());
 }
+
+
+class TestNodeWithoutUnknownOverwrite : public ::testing::Test
+{
+public:
+  TestNodeWithoutUnknownOverwrite()
+  {
+    node_ = std::make_shared<TestLifecycleNode>("obstacle_test_node");
+    node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
+    node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
+    node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
+    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3));
+    node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string("")));
+    // MaxWithoutUnknownOverwrite
+    node_->declare_parameter("obstacles.combination_method", rclcpp::ParameterValue(2));
+  }
+
+  ~TestNodeWithoutUnknownOverwrite() {}
+
+protected:
+  std::shared_ptr<TestLifecycleNode> node_;
+};
+
+/**
+ * Test CombinationMethod::MaxWithoutUnknownOverwrite in ObstacleLayer.
+ */
+TEST_F(TestNodeWithoutUnknownOverwrite, testMaxWithoutUnknownOverwriteCombinationMethod) {
+  tf2_ros::Buffer tf(node_->get_clock());
+
+  // Create a costmap with full unknown space
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
+
+  // The observation tries to set the cost of the cell to 254, but since it is unknown, it should
+  // remain unknown.
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+  // printMap(*(layers.getCostmap()));
+
+  int unknown_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::NO_INFORMATION);
+
+  ASSERT_EQ(unknown_count, 100);
+}

From 8237b29255f92ffdbff3ff957b495265e3908752 Mon Sep 17 00:00:00 2001
From: RBT22 <rozgonyibalint@gmail.com>
Date: Wed, 21 Jun 2023 09:29:46 +0200
Subject: [PATCH 11/11] Add CombinationMethod::Max test as a counter-case

---
 .../test/integration/obstacle_tests.cpp       | 24 +++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
index ca85d67eff8..ebd4c0c2d13 100644
--- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp
+++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
@@ -556,6 +556,30 @@ TEST_F(TestNode, testDynParamsSetStatic)
 }
 
 
+/**
+ * Test CombinationMethod::Max overwrites unknown value in ObstacleLayer.
+ */
+TEST_F(TestNode, testMaxCombinationMethod) {
+  tf2_ros::Buffer tf(node_->get_clock());
+
+  // Create a costmap with full unknown space
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
+
+  // The observation sets the cost of the cell to 254
+  layers.updateMap(0, 0, 0);  // 0, 0, 0 is robot pose
+  // printMap(*(layers.getCostmap()));
+
+  int unknown_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::NO_INFORMATION);
+
+  ASSERT_EQ(unknown_count, 99);
+}
+
 class TestNodeWithoutUnknownOverwrite : public ::testing::Test
 {
 public: