clear_tester.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <gtest/gtest.h>
00003 #include <clear_costmap_recovery/clear_costmap_recovery.h>
00004 
00005 #include <costmap_2d/testing_helper.h>
00006 
00007 tf::TransformListener* transformer;
00008 
00009 using costmap_2d::LETHAL_OBSTACLE;
00010 
00011 void testClearBehavior(std::string name, 
00012                        double distance, 
00013                        bool obstacles, 
00014                        bool static_map,
00015                        costmap_2d::Costmap2DROS* global_costmap, 
00016                        costmap_2d::Costmap2DROS* local_costmap){
00017     clear_costmap_recovery::ClearCostmapRecovery clear = clear_costmap_recovery::ClearCostmapRecovery();
00018     
00019     ros::NodeHandle clr("~/" + name);
00020     clr.setParam("reset_distance", distance);
00021     
00022     std::vector<std::string> clearable_layers;
00023     if(obstacles)
00024         clearable_layers.push_back( std::string("obstacles") );
00025     if(static_map)
00026         clearable_layers.push_back( std::string("static_map") );
00027         
00028     clr.setParam("layer_names", clearable_layers);
00029     
00030     clear.initialize(name, transformer, global_costmap, local_costmap);
00031     
00032     clear.runBehavior();
00033 }
00034 
00035 void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
00036 {
00037     costmap_2d::Costmap2DROS global(name + "/global", *transformer);
00038     costmap_2d::Costmap2DROS  local(name + "/local" , *transformer);
00039     boost::shared_ptr<costmap_2d::ObstacleLayer> olayer;
00040         
00041     std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global.getLayeredCostmap()->getPlugins();
00042     for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
00043         boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
00044         if(plugin->getName().find("obstacles")!=std::string::npos){
00045             olayer = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
00046             addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
00047             addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2);
00048         }
00049     }
00050     
00051     global.updateMap();
00052     local.updateMap();
00053     olayer->clearStaticObservations(true, true);
00054 
00055     testClearBehavior("clear", distance, obstacles, static_map, &global, &local);
00056     
00057     global.updateMap();
00058     local.updateMap();
00059     
00060     printMap(*global.getCostmap());
00061     ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal);
00062     ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE),  local_lethal);
00063     
00064 }
00065 
00066 TEST(ClearTester, basicTest){
00067   testCountLethal("base", 3.0, true, false, 20);
00068 }
00069 
00070 TEST(ClearTester, bigRadiusTest){
00071   testCountLethal("base", 20.0, true, false, 22);
00072 }
00073 
00074 TEST(ClearTester, clearNoLayersTest){
00075   testCountLethal("base", 20.0, false, false, 22);
00076 }
00077 
00078 TEST(ClearTester, clearBothTest){
00079   testCountLethal("base", 3.0, true, true, 0);
00080 }
00081 
00082 TEST(ClearTester, clearBothTest2){
00083   testCountLethal("base", 12.0, true, true, 6);
00084 }
00085 
00086 
00087 int main(int argc, char** argv){
00088   ros::init(argc, argv, "clear_tests");
00089   testing::InitGoogleTest(&argc, argv);
00090   transformer = new tf::TransformListener(ros::Duration(10));
00091   return RUN_ALL_TESTS();
00092 }


clear_costmap_recovery
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:55