git commit -m "first commit"

navigations/clear_costmap_recovery/test/clear_tester.cpp

@@ -0,0 +1,95 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <clear_costmap_recovery/clear_costmap_recovery.h>
+
+#include <costmap_2d/testing_helper.h>
+#include <tf2_ros/transform_listener.h>
+
+tf2_ros::Buffer* transformer;
+tf2_ros::TransformListener* tfl;
+
+using costmap_2d::LETHAL_OBSTACLE;
+
+void testClearBehavior(std::string name, 
+                       double distance, 
+                       bool obstacles, 
+                       bool static_map,
+                       costmap_2d::Costmap2DROS* global_costmap, 
+                       costmap_2d::Costmap2DROS* local_costmap){
+    clear_costmap_recovery::ClearCostmapRecovery clear = clear_costmap_recovery::ClearCostmapRecovery();
+    
+    ros::NodeHandle clr("~/" + name);
+    clr.setParam("reset_distance", distance);
+    
+    std::vector<std::string> clearable_layers;
+    if(obstacles)
+        clearable_layers.push_back( std::string("obstacles") );
+    if(static_map)
+        clearable_layers.push_back( std::string("static_map") );
+        
+    clr.setParam("layer_names", clearable_layers);
+    
+    clear.initialize(name, transformer, global_costmap, local_costmap);
+    
+    clear.runBehavior();
+}
+
+void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
+{
+    costmap_2d::Costmap2DROS global(name + "/global", *transformer);
+    costmap_2d::Costmap2DROS  local(name + "/local" , *transformer);
+    boost::shared_ptr<costmap_2d::ObstacleLayer> olayer;
+        
+    std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global.getLayeredCostmap()->getPlugins();
+    for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
+        boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
+        if(plugin->getName().find("obstacles")!=std::string::npos){
+            olayer = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
+            addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
+            addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2);
+        }
+    }
+    
+    global.updateMap();
+    local.updateMap();
+    olayer->clearStaticObservations(true, true);
+
+    testClearBehavior("clear", distance, obstacles, static_map, &global, &local);
+    
+    global.updateMap();
+    local.updateMap();
+    
+    printMap(*global.getCostmap());
+    ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal);
+    ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE),  local_lethal);
+    
+}
+
+TEST(ClearTester, basicTest){
+  testCountLethal("base", 3.0, true, false, 20);
+}
+
+TEST(ClearTester, bigRadiusTest){
+  testCountLethal("base", 20.0, true, false, 22);
+}
+
+TEST(ClearTester, clearNoLayersTest){
+  testCountLethal("base", 20.0, false, false, 22);
+}
+
+TEST(ClearTester, clearBothTest){
+  testCountLethal("base", 3.0, true, true, 0);
+}
+
+TEST(ClearTester, clearBothTest2){
+  testCountLethal("base", 12.0, true, true, 6);
+}
+
+
+int main(int argc, char** argv){
+  ros::init(argc, argv, "clear_tests");
+  testing::InitGoogleTest(&argc, argv);
+  transformer = new tf2_ros::Buffer(ros::Duration(10));
+  tfl = new tf2_ros::TransformListener(*transformer);
+  return RUN_ALL_TESTS();
+}