MapInflationTool.cpp
Go to the documentation of this file.
00001 #include <nav2d_navigator/MapInflationTool.h>
00002 
00003 CellData::CellData(double d, double i, unsigned int sx, unsigned int sy):distance(d), index(i), sx(sx), sy(sy){
00004 }
00005 
00006 MapInflationTool::MapInflationTool()
00007 {
00008         mInflationMarkers = NULL;
00009         mCachedCosts = NULL;
00010         mCachedDistances = NULL;
00011         mCostObstacle = 100;
00012 }
00013 
00014 MapInflationTool::~MapInflationTool()
00015 {
00016         delete[] mInflationMarkers;
00017         delete[] mCachedCosts;
00018         delete[] mCachedDistances;
00019 }
00020 
00021 // This method calculates two different lookup-tables:
00022 // 1. The distance of cells arount an obstacle towards this obstacle
00023 // 2. The cost that is applied to these cells around an obstacle
00024 void MapInflationTool::computeCaches(unsigned int radius)
00025 {
00026         mCellInflationRadius = radius;
00027         
00028         mCachedCosts = new char*[mCellInflationRadius + 2];
00029         mCachedDistances = new double*[mCellInflationRadius + 2];
00030         
00031         for(unsigned int i = 0; i < mCellInflationRadius + 2; i++)
00032         {
00033                 mCachedCosts[i] = new char[mCellInflationRadius + 2];
00034                 mCachedDistances[i] = new double[mCellInflationRadius + 2];
00035                 for(unsigned int j = 0; j < mCellInflationRadius + 2; j++)
00036                 {
00037                         double d = sqrt(i*i + j*j);
00038                         mCachedDistances[i][j] = d;
00039                         d /= mCellInflationRadius;
00040                         if(d > 1) d = 1;
00041                         mCachedCosts[i][j] = (1.0 - d) * mCostObstacle;
00042                 }
00043         }
00044 }
00045 
00046 // Method used to query the cache for a distance.
00047 inline double MapInflationTool::distanceLookup(int mx, int my, int src_x, int src_y)
00048 {
00049         unsigned int dx = abs(mx - src_x);
00050         unsigned int dy = abs(my - src_y);
00051         if(dx > mCellInflationRadius + 1 || dy > mCellInflationRadius + 1)
00052         {
00053                 ROS_ERROR("Error in distanceLookup! Asked for (%d, %d), but CellInflationRadius is %d!", dx, dy, mCellInflationRadius);
00054                 return 50.0;
00055         }
00056         return mCachedDistances[dx][dy];
00057 }
00058 
00059 // Method used to query the cache for the cell cost.
00060 inline char MapInflationTool::costLookup(int mx, int my, int src_x, int src_y)
00061 {
00062         unsigned int dx = abs(mx - src_x);
00063         unsigned int dy = abs(my - src_y);
00064         if(dx > mCellInflationRadius + 1 || dy > mCellInflationRadius + 1)
00065         {
00066                 ROS_ERROR("Error in costLookup! Asked for (%d, %d), but CellInflationRadius is %d!", dx, dy, mCellInflationRadius);
00067                 return 50.0;
00068         }
00069         return mCachedCosts[dx][dy];
00070 }
00071 
00072 // Main method to start the inflation on the given map in-place.
00073 void MapInflationTool::inflateMap(GridMap* map)
00074 {
00075         ROS_DEBUG("Started map inflation ...");
00076         
00077         // 0. Do some initialization
00078         mGridMap = map;
00079         int mapSize = mGridMap->getSize();
00080         
00081         if(mInflationMarkers) delete[] mInflationMarkers;
00082         mInflationMarkers = new unsigned char[mapSize];
00083         for(int i = 0; i < mapSize; i++)
00084         {
00085                 mInflationMarkers[i] = 0;
00086         }
00087         
00088         // 1. Put all real obstacles in a queue
00089         while(!mInflationQueue.empty()) mInflationQueue.pop();
00090         
00091         for(int index = 0; index < mapSize; index++)
00092         {
00093                 if(mGridMap->getData(index) > 0)
00094                 {
00095                         unsigned int sx, sy;
00096                         mGridMap->getCoordinates(sx, sy, index);
00097                         enqueueObstacle(index, sx, sy);
00098                 }else if(mGridMap->getData(index) == -1)
00099                 {
00100                         mInflationMarkers[index] = 1;
00101                 } 
00102         }
00103         
00104         // 2. Inflate them by the given inflation radius
00105         int count = 0;
00106         while(!mInflationQueue.empty())
00107         {
00108                 CellData cell = mInflationQueue.top();
00109                 mInflationQueue.pop();
00110                 unsigned int x,y;
00111                 if(!mGridMap->getCoordinates(x, y, cell.index)) continue;
00112                 if(x >= 1)
00113                         enqueueObstacle(cell.index-1, cell.sx, cell.sy);
00114                 if(x < mGridMap->getWidth() - 1)        
00115                         enqueueObstacle(cell.index+1, cell.sx, cell.sy);
00116                 if(y >= 1)
00117                         enqueueObstacle(cell.index-mGridMap->getWidth(), cell.sx, cell.sy);
00118                 if(y < mGridMap->getHeight() - 1)
00119                         enqueueObstacle(cell.index+mGridMap->getWidth(), cell.sx, cell.sy);
00120                 count++;
00121         }
00122         
00123         ROS_DEBUG("Finished inflation. (%d cells)", count);
00124 }
00125 
00126 void MapInflationTool::enqueueObstacle(unsigned int index, unsigned int sx, unsigned int sy)
00127 {
00128         unsigned int mx, my;
00129         if(!mGridMap->getCoordinates(mx, my, index)) return;
00130         if(mInflationMarkers[index] != 0) return;
00131         
00132         double distance = distanceLookup(mx, my, sx, sy);
00133         if(distance == 50)
00134                 ROS_INFO("Tried to add cell (%u, %u) -> (%u, %u) to inflation queue!", sx, sy, mx, my);
00135         
00136         if(distance > mCellInflationRadius) return;
00137                 
00138         CellData cell(distance, index, sx, sy);
00139         mInflationQueue.push(cell);
00140         mInflationMarkers[index] = 1;
00141         char value = costLookup(mx, my, sx, sy);
00142         mGridMap->setData(index, value);
00143 //      ROS_DEBUG("Set cell %d cost to %d", index, value);
00144 }


nav2d_navigator
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:13