Go to the documentation of this file.00001 #ifndef TESTING_HELPER_H
00002 #define 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, double ox=0.0, double oy=0.0, double oz=MAX_Z){
00076 pcl::PointCloud<pcl::PointXYZ> cloud;
00077 cloud.points.resize(1);
00078 cloud.points[0].x = x;
00079 cloud.points[0].y = y;
00080 cloud.points[0].z = z;
00081
00082 geometry_msgs::Point p;
00083 p.x = ox;
00084 p.y = oy;
00085 p.z = oz;
00086
00087 costmap_2d::Observation obs(p, cloud, 100.0, 100.0);
00088 olayer->addStaticObservation(obs, true, true);
00089 }
00090
00091 costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf::TransformListener& tf)
00092 {
00093 costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
00094 ilayer->initialize(&layers, "inflation", &tf);
00095 boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
00096 layers.addPlugin( ipointer );
00097 return ilayer;
00098 }
00099
00100
00101 #endif
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:55