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

…os-navigation#3612)

* 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

@@ -136,6 +136,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,
@@ -216,4 +245,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);
+}