testing_helper.h
Go to the documentation of this file.
00001 #ifndef COSTMAP_2D_TESTING_HELPER_H
00002 #define COSTMAP_2D_TESTING_HELPER_H
00003 
00004 #include<costmap_2d/cost_values.h>
00005 #include<costmap_2d/costmap_2d.h>
00006 #include <costmap_2d/static_layer.h>
00007 #include <costmap_2d/obstacle_layer.h>
00008 #include <costmap_2d/inflation_layer.h>
00009 
00010 const double MAX_Z(1.0);
00011 
00012 void setValues(costmap_2d::Costmap2D& costmap, const unsigned char* map)
00013 {
00014   int index = 0;
00015   for (int i = 0; i < costmap.getSizeInCellsY(); i++){
00016     for (int j = 0; j < costmap.getSizeInCellsX(); j++){
00017       costmap.setCost(j, i, map[index]);
00018     }
00019   }
00020 }
00021 
00022 char printableCost(unsigned char cost)
00023 {
00024   switch (cost)
00025   {
00026   case costmap_2d::NO_INFORMATION: return '?';
00027   case costmap_2d::LETHAL_OBSTACLE: return 'L';
00028   case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
00029   case costmap_2d::FREE_SPACE: return '.';
00030   default: return '0' + (unsigned char) (10 * cost / 255);
00031   }
00032 }
00033 
00034 void printMap(costmap_2d::Costmap2D& costmap)
00035 {
00036   printf("map:\n");
00037   for (int i = 0; i < costmap.getSizeInCellsY(); i++){
00038     for (int j = 0; j < costmap.getSizeInCellsX(); j++){
00039       printf("%4d", int(costmap.getCost(j, i)));
00040     }
00041     printf("\n\n");
00042   }
00043 }
00044 
00045 unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
00046 {
00047   unsigned int count = 0;
00048   for (int i = 0; i < costmap.getSizeInCellsY(); i++){
00049     for (int j = 0; j < costmap.getSizeInCellsX(); j++){
00050       unsigned char c = costmap.getCost(j, i);
00051       if ((equal && c == value) || (!equal && c != value))
00052       {
00053         count+=1;
00054       }
00055     }
00056   }
00057   return count;
00058 }
00059 
00060 void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf)
00061 {
00062   costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
00063   layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
00064   slayer->initialize(&layers, "static", &tf);
00065 }
00066 
00067 costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf)
00068 {
00069   costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
00070   olayer->initialize(&layers, "obstacles", &tf);
00071   layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
00072   return olayer;
00073 }
00074 
00075 void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
00076                     double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
00077   pcl::PointCloud<pcl::PointXYZ> cloud;
00078   cloud.points.resize(1);
00079   cloud.points[0].x = x;
00080   cloud.points[0].y = y;
00081   cloud.points[0].z = z;
00082 
00083   geometry_msgs::Point p;
00084   p.x = ox;
00085   p.y = oy;
00086   p.z = oz;
00087 
00088   costmap_2d::Observation obs(p, cloud, 100.0, 100.0);  // obstacle range = raytrace range = 100.0
00089   olayer->addStaticObservation(obs, true, true);
00090 }
00091 
00092 costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf)
00093 {
00094   costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
00095   ilayer->initialize(&layers, "inflation", &tf);
00096   boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
00097   layers.addPlugin(ipointer);
00098   return ilayer;
00099 }
00100 
00101 
00102 #endif  // COSTMAP_2D_TESTING_HELPER_H


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15