GridMap.h
Go to the documentation of this file.
00001 #ifndef GRID_MAP_H
00002 #define GRID_MAP_H
00003 
00004 #include "ros/ros.h"
00005 #include "nav_msgs/OccupancyGrid.h"
00006 
00007 class GridMap
00008 {
00009 public:
00010         void update(nav_msgs::OccupancyGrid grid)
00011         {
00012                 mOccupancyGrid = grid;
00013                 mMapWidth = mOccupancyGrid.info.width;
00014                 mMapHeight = mOccupancyGrid.info.height;
00015                 ROS_DEBUG("Got new map of size %d x %d", mMapWidth, mMapHeight);
00016         }
00017 
00018         unsigned int getWidth() {return mMapWidth;}
00019         unsigned int getHeight() {return mMapHeight;}
00020         unsigned int getSize() {return mMapWidth * mMapHeight;}
00021         double getResolution() {return mOccupancyGrid.info.resolution;}
00022         double getOriginX() {return mOccupancyGrid.info.origin.position.x;}
00023         double getOriginY() {return mOccupancyGrid.info.origin.position.y;}
00024         
00025         char getLethalCost(){return mLethalCost;}
00026         void setLethalCost(char c){mLethalCost = c;}
00027         
00028         const nav_msgs::OccupancyGrid& getMap() const {return mOccupancyGrid;}
00029         
00030         // Get the array index from the given x,y coordinates
00031         bool getIndex(unsigned int x, unsigned int y, unsigned int &i)
00032         {
00033                 if(x >= mMapWidth || y >= mMapHeight)
00034                 {
00035                         return false;
00036                 }
00037                 i = y * mMapWidth + x;
00038                 return true;
00039         }
00040         
00041         // Get the x,y coordinates from the given array index
00042         bool getCoordinates(unsigned int &x, unsigned int &y, unsigned int i)
00043         {
00044                 if(i >= mMapWidth * mMapHeight)
00045                 {
00046                         ROS_ERROR("getCoords() failed!");
00047                         return false;
00048                 }
00049                 y = i / mMapWidth;
00050                 x = i % mMapWidth;
00051                 return true;
00052         }
00053         
00054         // Index based methods
00055         char getData(unsigned int index)
00056         {
00057                 if(index < mMapWidth * mMapHeight)
00058                         return mOccupancyGrid.data[index];
00059                 else
00060                         return -1;
00061         }
00062         
00063         bool setData(unsigned int index, char value)
00064         {
00065                 if(index >= mMapWidth * mMapHeight)
00066                 {
00067                         return false;
00068                 }
00069                 mOccupancyGrid.data[index] = value;
00070                 return true;
00071         }
00072 
00073         bool isFree(unsigned int index)
00074         {
00075                 char value = getData(index);
00076                 if(value >= 0 && value < mLethalCost) return true;
00077                 return false;
00078         }
00079 
00080         bool isFrontier(unsigned int index)
00081         {
00082                 int y = index / mMapWidth;
00083                 int x = index % mMapWidth;
00084                 
00085                 if(getData(x-1, y-1) == -1) return true;
00086                 if(getData(x-1, y  ) == -1) return true;
00087                 if(getData(x-1, y+1) == -1) return true;
00088                 if(getData(x  , y-1) == -1) return true;
00089                 if(getData(x  , y+1) == -1) return true;
00090                 if(getData(x+1, y-1) == -1) return true;
00091                 if(getData(x+1, y  ) == -1) return true;
00092                 if(getData(x+1, y+1) == -1) return true;
00093                 
00094                 return false;
00095         }
00096         
00098         std::vector<unsigned int> getFreeNeighbors(unsigned int index, int offset = 1)
00099         {
00100                 std::vector<unsigned int> neighbors;
00101                 
00102                 if(offset < 0) offset *= -1;
00103                 int y = index / mMapWidth;
00104                 int x = index % mMapWidth;
00105                 
00106                 for(int i = -offset; i <= offset; i++)
00107                         for(int j = -offset; j <= offset; j++)
00108                                 if(getIndex(x+i, y+j, index) && isFree(index))
00109                                         neighbors.push_back(index);
00110                                         
00111                 return neighbors;
00112         }
00113 
00115         std::vector<unsigned int> getNeighbors(unsigned int index, bool diagonal = false)
00116         {
00117                 std::vector<unsigned int> neighbors;
00118         
00119                 int y = index / mMapWidth;
00120                 int x = index % mMapWidth;
00121                 unsigned int i;
00122                 if(getIndex(x-1,y,  i)) neighbors.push_back(i);
00123                 if(getIndex(x+1,y,  i)) neighbors.push_back(i);
00124                 if(getIndex(x,  y-1,i)) neighbors.push_back(i);
00125                 if(getIndex(x,  y+1,i)) neighbors.push_back(i);
00126                 
00127                 if(diagonal)
00128                 {
00129                         if(getIndex(x-1,y-1,i)) neighbors.push_back(i);
00130                         if(getIndex(x-1,y+1,i)) neighbors.push_back(i);
00131                         if(getIndex(x+1,y-1,i)) neighbors.push_back(i);
00132                         if(getIndex(x+1,y+1,i)) neighbors.push_back(i);
00133                 }
00134                 return neighbors;
00135         }
00136 
00137         // Coordinate based methods
00138         char getData(int x, int y)
00139         {
00140                 if(x < 0 ||x >= (int)mMapWidth || y < 0 || y >= (int)mMapHeight)
00141                         return -1;
00142                 else
00143                         return mOccupancyGrid.data[y*mMapWidth + x];
00144         }
00145         
00146         bool setData(int x, int y, char value)
00147         {
00148                 if(x < 0 ||x >= (int)mMapWidth || y < 0 || y >= (int)mMapHeight)
00149                 {
00150                         return false;
00151                 }
00152                 mOccupancyGrid.data[y*mMapWidth + x] = value;
00153                 return true;
00154         }
00155         
00156         bool isFree(int x, int y)
00157         {
00158                 char value = getData(x, y);
00159                 if(value >= 0 && value < mLethalCost) return true;
00160                 return false;
00161         }
00162 private:
00163 
00164         nav_msgs::OccupancyGrid mOccupancyGrid;
00165         unsigned int mMapWidth;
00166         unsigned int mMapHeight;
00167         char mLethalCost;
00168 };
00169 
00170 #endif


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:43