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();
+}

navigations/clear_costmap_recovery/test/clear_tests.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
+  <node name="static_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /base_link 100" />
+  <test time-limit="300" test-name="clear_tests" pkg="clear_costmap_recovery" type="clear_tester" />
+  <rosparam command="load" file="$(find clear_costmap_recovery)/test/params.yaml" ns="clear_tests"/>
+</launch>

navigations/clear_costmap_recovery/test/params.yaml

@@ -0,0 +1,13 @@
+base:
+    global:
+        plugins:
+        - {name: static_map,       type: "costmap_2d::StaticLayer"}
+        - {name: obstacles,        type: "costmap_2d::ObstacleLayer"}
+        - {name: inflation,        type: "costmap_2d::InflationLayer"}
+        obstacles:
+            track_unknown_space: true
+    local:
+        plugins: []
+    robot_radius: .5
+clear:
+    clearing_distance: 7.0