clear_tester.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <gtest/gtest.h>
4 
6 
8 
10 
11 void testClearBehavior(std::string name,
12  double distance,
13  bool obstacles,
14  bool static_map,
15  costmap_2d::Costmap2DROS* global_costmap,
16  costmap_2d::Costmap2DROS* local_costmap){
18 
19  ros::NodeHandle clr("~/" + name);
20  clr.setParam("reset_distance", distance);
21 
22  std::vector<std::string> clearable_layers;
23  if(obstacles)
24  clearable_layers.push_back( std::string("obstacles") );
25  if(static_map)
26  clearable_layers.push_back( std::string("static_map") );
27 
28  clr.setParam("layer_names", clearable_layers);
29 
30  clear.initialize(name, transformer, global_costmap, local_costmap);
31 
32  clear.runBehavior();
33 }
34 
35 void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
36 {
37  costmap_2d::Costmap2DROS global(name + "/global", *transformer);
38  costmap_2d::Costmap2DROS local(name + "/local" , *transformer);
40 
41  std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global.getLayeredCostmap()->getPlugins();
42  for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
43  boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
44  if(plugin->getName().find("obstacles")!=std::string::npos){
45  olayer = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
46  addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
47  addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2);
48  }
49  }
50 
51  global.updateMap();
52  local.updateMap();
53  olayer->clearStaticObservations(true, true);
54 
55  testClearBehavior("clear", distance, obstacles, static_map, &global, &local);
56 
57  global.updateMap();
58  local.updateMap();
59 
60  printMap(*global.getCostmap());
61  ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal);
62  ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE), local_lethal);
63 
64 }
65 
66 TEST(ClearTester, basicTest){
67  testCountLethal("base", 3.0, true, false, 20);
68 }
69 
70 TEST(ClearTester, bigRadiusTest){
71  testCountLethal("base", 20.0, true, false, 22);
72 }
73 
74 TEST(ClearTester, clearNoLayersTest){
75  testCountLethal("base", 20.0, false, false, 22);
76 }
77 
78 TEST(ClearTester, clearBothTest){
79  testCountLethal("base", 3.0, true, true, 0);
80 }
81 
82 TEST(ClearTester, clearBothTest2){
83  testCountLethal("base", 12.0, true, true, 6);
84 }
85 
86 
87 int main(int argc, char** argv){
88  ros::init(argc, argv, "clear_tests");
89  testing::InitGoogleTest(&argc, argv);
90  transformer = new tf::TransformListener(ros::Duration(10));
91  return RUN_ALL_TESTS();
92 }
unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal=true)
tf::TransformListener * transformer
Definition: clear_tester.cpp:7
void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
void testClearBehavior(std::string name, double distance, bool obstacles, bool static_map, costmap_2d::Costmap2DROS *global_costmap, costmap_2d::Costmap2DROS *local_costmap)
const double MAX_Z(1.0)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< boost::shared_ptr< Layer > > * getPlugins()
A recovery behavior that reverts the navigation stack&#39;s costmaps to the static map outside of a user-...
int main(int argc, char **argv)
void runBehavior()
Run the ClearCostmapRecovery recovery behavior. Reverts the costmap to the static map outside of a us...
static const unsigned char LETHAL_OBSTACLE
void addObservation(costmap_2d::ObstacleLayer *olayer, double x, double y, double z=0.0, double ox=0.0, double oy=0.0, double oz=MAX_Z)
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *global_costmap, costmap_2d::Costmap2DROS *local_costmap)
Initialization function for the ClearCostmapRecovery recovery behavior.
Costmap2D * getCostmap()
void printMap(costmap_2d::Costmap2D &costmap)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
LayeredCostmap * getLayeredCostmap()
TEST(ClearTester, basicTest)


clear_costmap_recovery
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:33