Option for ObstacleLayer to not override StaticLayer's unknown parts (r…

…os-navigation#3612) (#6)

* Add updateWithMaxWithoutUnknownOverride

* Add missing break to switch case

* Add additional NO_INFORMATION check to make more robust

* Add CombinationMethod enum with combination_method_from_int

* Rename override to overwrite

* Update docs of combination_method_from_int

* Move definitions to costmap_layer and remove function_name param

* Replace logger with node's logger

* Fix linting errors

* Add test

* Add CombinationMethod::Max test as a counter-case

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;

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 updateWithMaxWithoutUnknownOverwrite(
+    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.
@@ -172,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_;
 };

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -252,7 +252,7 @@ class ObstacleLayer : public CostmapLayer
 
   bool rolling_window_;
   bool was_reset_;
-  int combination_method_;
+  nav2_costmap_2d::CombinationMethod combination_method_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -92,11 +92,14 @@ 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);
+
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(
       &ObstacleLayer::dynamicParametersCallback,
@@ -311,7 +314,7 @@ 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());
       }
     }
   }
@@ -540,12 +543,15 @@ 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 CombinationMethod::MaxWithoutUnknownOverwrite:
+      updateWithMaxWithoutUnknownOverwrite(master_grid, min_i, min_j, max_i, max_j);
+      break;
     default:  // Nothing
       break;
   }

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -86,9 +86,12 @@ 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);
+
   auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
 
   if (publish_voxel_) {
@@ -522,7 +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_ = parameter.as_int();
+        combination_method_ = combination_method_from_int(parameter.as_int());
       }
     }
   }

nav2_costmap_2d/src/costmap_layer.cpp

@@ -135,6 +135,35 @@ void CostmapLayer::updateWithMax(
   }
 }
 
+void CostmapLayer::updateWithMaxWithoutUnknownOverwrite(
+  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,
   int min_j,
@@ -215,4 +244,23 @@ 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(
+        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);
+      return CombinationMethod::Max;
+  }
+}
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -554,3 +554,74 @@ TEST_F(TestNode, testDynParamsSetStatic)
   costmap->on_cleanup(rclcpp_lifecycle::State());
   costmap->on_shutdown(rclcpp_lifecycle::State());
 }
+
+
+/**
+ * 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:
+  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);
+}