added clearArea test

Signed-off-by: alexander <[email protected]>

nav2_costmap_2d/test/integration/plugin_container_tests.cpp

@@ -397,6 +397,75 @@ TEST_F(TestNode, testResetting) {
   ASSERT_EQ(ilayer_b->isCurrent(), false);
 }
 
+TEST_F(TestNode, testClearing) {
+  tf2_ros::Buffer tf(node_->get_clock());
+
+  node_->declare_parameter("pclayer_a.static.map_topic",
+    rclcpp::ParameterValue(std::string("map")));
+
+  node_->declare_parameter("pclayer_a.inflation.cost_scaling_factor",
+    rclcpp::ParameterValue(1.0));
+
+  node_->declare_parameter("pclayer_a.inflation.inflation_radius",
+    rclcpp::ParameterValue(1.0));
+
+  node_->declare_parameter("pclayer_b.inflation.cost_scaling_factor",
+    rclcpp::ParameterValue(1.0));
+
+  node_->declare_parameter("pclayer_b.inflation.inflation_radius",
+    rclcpp::ParameterValue(1.0));
+
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+
+  layers.resizeMap(10, 10, 1, 0, 0);
+
+  std::shared_ptr<nav2_costmap_2d::PluginContainerLayer> pclayer_a = nullptr;
+  addPluginContainerLayer(layers, tf, node_, pclayer_a, "pclayer_a");
+
+  std::shared_ptr<nav2_costmap_2d::PluginContainerLayer> pclayer_b = nullptr;
+  addPluginContainerLayer(layers, tf, node_, pclayer_b, "pclayer_b");
+
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer =
+    std::make_shared<nav2_costmap_2d::StaticLayer>();
+  pclayer_a->addPlugin(slayer, "pclayer_a.static");
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer_a =
+    std::make_shared<nav2_costmap_2d::InflationLayer>();
+  pclayer_a->addPlugin(ilayer_a, "pclayer_a.inflation");
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer_b =
+    std::make_shared<nav2_costmap_2d::ObstacleLayer>();
+  pclayer_b->addPlugin(olayer_b, "pclayer_b.obstacles");
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer_b =
+    std::make_shared<nav2_costmap_2d::InflationLayer>();
+  pclayer_b->addPlugin(ilayer_b, "pclayer_a.inflation");
+
+  std::vector<geometry_msgs::msg::Point> polygon = setRadii(layers, 1, 1);
+  layers.setFootprint(polygon);
+
+  addObservation(olayer_b, 9.0, 9.0);
+
+  waitForMap(slayer);
+
+  layers.updateMap(0, 0, 0);
+
+  nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 29);
+
+  pclayer_a->clearArea(-1, -1, 10, 10, false);
+  pclayer_b->clearArea(-1, -1, 10, 10, false);
+
+  layers.updateMap(0, 0, 0);
+
+  costmap = layers.getCostmap();
+
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1);
+  ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 2);
+}
+
 TEST_F(TestNode, testOverwriteCombinationMethods) {
   tf2_ros::Buffer tf(node_->get_clock());