testing_helper.h
Go to the documentation of this file.
1 #ifndef COSTMAP_2D_TESTING_HELPER_H
2 #define COSTMAP_2D_TESTING_HELPER_H
3 
9 
10 const double MAX_Z(1.0);
11 
12 void setValues(costmap_2d::Costmap2D& costmap, const unsigned char* map)
13 {
14  int index = 0;
15  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
16  for (int j = 0; j < costmap.getSizeInCellsX(); j++){
17  costmap.setCost(j, i, map[index]);
18  }
19  }
20 }
21 
22 char printableCost(unsigned char cost)
23 {
24  switch (cost)
25  {
26  case costmap_2d::NO_INFORMATION: return '?';
27  case costmap_2d::LETHAL_OBSTACLE: return 'L';
29  case costmap_2d::FREE_SPACE: return '.';
30  default: return '0' + (unsigned char) (10 * cost / 255);
31  }
32 }
33 
35 {
36  printf("map:\n");
37  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
38  for (int j = 0; j < costmap.getSizeInCellsX(); j++){
39  printf("%4d", int(costmap.getCost(j, i)));
40  }
41  printf("\n\n");
42  }
43 }
44 
45 unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
46 {
47  unsigned int count = 0;
48  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
49  for (int j = 0; j < costmap.getSizeInCellsX(); j++){
50  unsigned char c = costmap.getCost(j, i);
51  if ((equal && c == value) || (!equal && c != value))
52  {
53  count+=1;
54  }
55  }
56  }
57  return count;
58 }
59 
61 {
64  slayer->initialize(&layers, "static", &tf);
65 }
66 
68 {
70  olayer->initialize(&layers, "obstacles", &tf);
72  return olayer;
73 }
74 
75 void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
76  double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
77  pcl::PointCloud<pcl::PointXYZ> cloud;
78  cloud.points.resize(1);
79  cloud.points[0].x = x;
80  cloud.points[0].y = y;
81  cloud.points[0].z = z;
82 
83  geometry_msgs::Point p;
84  p.x = ox;
85  p.y = oy;
86  p.z = oz;
87 
88  costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0
89  olayer->addStaticObservation(obs, true, true);
90 }
91 
93 {
95  ilayer->initialize(&layers, "inflation", &tf);
96  boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
97  layers.addPlugin(ipointer);
98  return ilayer;
99 }
100 
101 
102 #endif // COSTMAP_2D_TESTING_HELPER_H
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
Set the cost of a cell in the costmap.
Definition: costmap_2d.cpp:197
unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal=true)
const double MAX_Z(1.0)
void initialize(LayeredCostmap *parent, std::string name, tf::TransformListener *tf)
Definition: layer.cpp:43
char printableCost(unsigned char cost)
costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
Stores an observation in terms of a point cloud and the origin of the source.
Definition: observation.h:47
void addStaticLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char FREE_SPACE
Definition: cost_values.h:45
void addStaticObservation(costmap_2d::Observation &obs, bool marking, bool clearing)
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
Definition: costmap_2d.cpp:192
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
Definition: cost_values.h:44
void addPlugin(boost::shared_ptr< Layer > plugin)
void setValues(costmap_2d::Costmap2D &costmap, const unsigned char *map)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Definition: costmap_2d.cpp:431
TFSIMD_FORCE_INLINE const tfScalar & z() const
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
Definition: costmap_2d.cpp:426
static const unsigned char LETHAL_OBSTACLE
Definition: cost_values.h:43
costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char NO_INFORMATION
Definition: cost_values.h:42
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)
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:60
void printMap(costmap_2d::Costmap2D &costmap)
Instantiates different layer plugins and aggregates them into one score.


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42