GridMap.h
Go to the documentation of this file.
1 #ifndef GRID_MAP_H
2 #define GRID_MAP_H
3 
4 #include "ros/ros.h"
5 #include "nav_msgs/OccupancyGrid.h"
6 
7 class GridMap
8 {
9 public:
10  void update(nav_msgs::OccupancyGrid grid)
11  {
12  mOccupancyGrid = grid;
13  mMapWidth = mOccupancyGrid.info.width;
14  mMapHeight = mOccupancyGrid.info.height;
15  ROS_DEBUG("Got new map of size %d x %d", mMapWidth, mMapHeight);
16  }
17 
18  unsigned int getWidth() {return mMapWidth;}
19  unsigned int getHeight() {return mMapHeight;}
20  unsigned int getSize() {return mMapWidth * mMapHeight;}
21  double getResolution() {return mOccupancyGrid.info.resolution;}
22  double getOriginX() {return mOccupancyGrid.info.origin.position.x;}
23  double getOriginY() {return mOccupancyGrid.info.origin.position.y;}
24 
25  signed char getLethalCost(){return mLethalCost;}
26  void setLethalCost(signed char c){mLethalCost = c;}
27 
28  const nav_msgs::OccupancyGrid& getMap() const {return mOccupancyGrid;}
29 
30  // Get the array index from the given x,y coordinates
31  bool getIndex(unsigned int x, unsigned int y, unsigned int &i)
32  {
33  if(x >= mMapWidth || y >= mMapHeight)
34  {
35  return false;
36  }
37  i = y * mMapWidth + x;
38  return true;
39  }
40 
41  // Get the x,y coordinates from the given array index
42  bool getCoordinates(unsigned int &x, unsigned int &y, unsigned int i)
43  {
44  if(i >= mMapWidth * mMapHeight)
45  {
46  ROS_ERROR("getCoords() failed!");
47  return false;
48  }
49  y = i / mMapWidth;
50  x = i % mMapWidth;
51  return true;
52  }
53 
54  // Index based methods
55  signed char getData(unsigned int index)
56  {
57  if(index < mMapWidth * mMapHeight)
58  return mOccupancyGrid.data[index];
59  else
60  return -1;
61  }
62 
63  bool setData(unsigned int index, signed char value)
64  {
65  if(index >= mMapWidth * mMapHeight)
66  {
67  return false;
68  }
69  mOccupancyGrid.data[index] = value;
70  return true;
71  }
72 
73  bool isFree(unsigned int index)
74  {
75  signed char value = getData(index);
76  if(value >= 0 && value < mLethalCost) return true;
77  return false;
78  }
79 
80  bool isFrontier(unsigned int index)
81  {
82  int y = index / mMapWidth;
83  int x = index % mMapWidth;
84 
85  if(getData(x-1, y-1) == -1) return true;
86  if(getData(x-1, y ) == -1) return true;
87  if(getData(x-1, y+1) == -1) return true;
88  if(getData(x , y-1) == -1) return true;
89  if(getData(x , y+1) == -1) return true;
90  if(getData(x+1, y-1) == -1) return true;
91  if(getData(x+1, y ) == -1) return true;
92  if(getData(x+1, y+1) == -1) return true;
93 
94  return false;
95  }
96 
98  std::vector<unsigned int> getFreeNeighbors(unsigned int index, int offset = 1)
99  {
100  std::vector<unsigned int> neighbors;
101 
102  if(offset < 0) offset *= -1;
103  int y = index / mMapWidth;
104  int x = index % mMapWidth;
105 
106  for(int i = -offset; i <= offset; i++)
107  for(int j = -offset; j <= offset; j++)
108  if(getIndex(x+i, y+j, index) && isFree(index))
109  neighbors.push_back(index);
110 
111  return neighbors;
112  }
113 
115  std::vector<unsigned int> getNeighbors(unsigned int index, bool diagonal = false)
116  {
117  std::vector<unsigned int> neighbors;
118 
119  int y = index / mMapWidth;
120  int x = index % mMapWidth;
121  unsigned int i;
122  if(getIndex(x-1,y, i)) neighbors.push_back(i);
123  if(getIndex(x+1,y, i)) neighbors.push_back(i);
124  if(getIndex(x, y-1,i)) neighbors.push_back(i);
125  if(getIndex(x, y+1,i)) neighbors.push_back(i);
126 
127  if(diagonal)
128  {
129  if(getIndex(x-1,y-1,i)) neighbors.push_back(i);
130  if(getIndex(x-1,y+1,i)) neighbors.push_back(i);
131  if(getIndex(x+1,y-1,i)) neighbors.push_back(i);
132  if(getIndex(x+1,y+1,i)) neighbors.push_back(i);
133  }
134  return neighbors;
135  }
136 
137  // Coordinate based methods
138  signed char getData(int x, int y)
139  {
140  if(x < 0 ||x >= (int)mMapWidth || y < 0 || y >= (int)mMapHeight)
141  return -1;
142  else
143  return mOccupancyGrid.data[y*mMapWidth + x];
144  }
145 
146  bool setData(int x, int y, signed char value)
147  {
148  if(x < 0 ||x >= (int)mMapWidth || y < 0 || y >= (int)mMapHeight)
149  {
150  return false;
151  }
152  mOccupancyGrid.data[y*mMapWidth + x] = value;
153  return true;
154  }
155 
156  bool isFree(int x, int y)
157  {
158  signed char value = getData(x, y);
159  if(value >= 0 && value < mLethalCost) return true;
160  return false;
161  }
162 private:
163 
164  nav_msgs::OccupancyGrid mOccupancyGrid;
165  unsigned int mMapWidth;
166  unsigned int mMapHeight;
167  signed char mLethalCost;
168 };
169 
170 #endif
double getResolution()
Definition: GridMap.h:21
bool setData(unsigned int index, signed char value)
Definition: GridMap.h:63
nav_msgs::OccupancyGrid mOccupancyGrid
Definition: GridMap.h:164
bool isFree(int x, int y)
Definition: GridMap.h:156
Definition: GridMap.h:7
bool setData(int x, int y, signed char value)
Definition: GridMap.h:146
double getOriginX()
Definition: GridMap.h:22
void update(nav_msgs::OccupancyGrid grid)
Definition: GridMap.h:10
unsigned int getHeight()
Definition: GridMap.h:19
TFSIMD_FORCE_INLINE const tfScalar & y() const
unsigned int getSize()
Definition: GridMap.h:20
signed char getLethalCost()
Definition: GridMap.h:25
signed char getData(unsigned int index)
Definition: GridMap.h:55
std::vector< unsigned int > getNeighbors(unsigned int index, bool diagonal=false)
Definition: GridMap.h:115
bool isFree(unsigned int index)
Definition: GridMap.h:73
const nav_msgs::OccupancyGrid & getMap() const
Definition: GridMap.h:28
TFSIMD_FORCE_INLINE const tfScalar & x() const
unsigned int getWidth()
Definition: GridMap.h:18
unsigned int mMapHeight
Definition: GridMap.h:166
signed char mLethalCost
Definition: GridMap.h:167
unsigned int mMapWidth
Definition: GridMap.h:165
bool isFrontier(unsigned int index)
Definition: GridMap.h:80
bool getIndex(unsigned int x, unsigned int y, unsigned int &i)
Definition: GridMap.h:31
signed char getData(int x, int y)
Definition: GridMap.h:138
#define ROS_ERROR(...)
std::vector< unsigned int > getFreeNeighbors(unsigned int index, int offset=1)
Definition: GridMap.h:98
double getOriginY()
Definition: GridMap.h:23
void setLethalCost(signed char c)
Definition: GridMap.h:26
bool getCoordinates(unsigned int &x, unsigned int &y, unsigned int i)
Definition: GridMap.h:42
#define ROS_DEBUG(...)


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:43