NearestFrontierPlanner.cpp
Go to the documentation of this file.
00001 #include "NearestFrontierPlanner.h"
00002 
00003 typedef std::multimap<double,unsigned int> Queue;
00004 typedef std::pair<double,unsigned int> Entry;
00005 
00006 NearestFrontierPlanner::NearestFrontierPlanner()
00007 {
00008         
00009 }
00010 
00011 NearestFrontierPlanner::~NearestFrontierPlanner()
00012 {
00013         
00014 }
00015 
00016 int NearestFrontierPlanner::findExplorationTarget(GridMap* map, unsigned int start, unsigned int &goal)
00017 {
00018         // Create some workspace for the wavefront algorithm
00019         unsigned int mapSize = map->getSize();
00020         double* plan = new double[mapSize];
00021         for(unsigned int i = 0; i < mapSize; i++)
00022         {
00023                 plan[i] = -1;
00024         }
00025         
00026         // Initialize the queue with the robot position
00027         Queue queue;
00028         Entry startPoint(0.0, start);
00029         queue.insert(startPoint);
00030         plan[start] = 0;
00031         
00032         Queue::iterator next;
00033         double distance;
00034         double linear = map->getResolution();
00035         bool foundFrontier = false;
00036         int cellCount = 0;
00037         
00038         // Do full search with weightless Dijkstra-Algorithm
00039         while(!queue.empty())
00040         {
00041                 cellCount++;
00042                 // Get the nearest cell from the queue
00043                 next = queue.begin();
00044                 distance = next->first;
00045                 unsigned int index = next->second;
00046                 queue.erase(next);
00047                 
00048                 // Add all adjacent cells
00049                 if(map->isFrontier(index))
00050                 {
00051                         // We reached the border of the map, which is unexplored terrain as well:
00052                         foundFrontier = true;
00053                         goal = index;
00054                         break;
00055                 }else
00056                 {
00057                         unsigned int ind[4];
00058 
00059                         ind[0] = index - 1;               // left
00060                         ind[1] = index + 1;               // right
00061                         ind[2] = index - map->getWidth(); // up
00062                         ind[3] = index + map->getWidth(); // down
00063                         
00064                         for(unsigned int it = 0; it < 4; it++)
00065                         {
00066                                 unsigned int i = ind[it];
00067                                 if(map->isFree(i) && plan[i] == -1)
00068                                 {
00069                                         queue.insert(Entry(distance+linear, i));
00070                                         plan[i] = distance+linear;
00071                                 }
00072                         }
00073                 }
00074         }
00075 
00076         ROS_DEBUG("Checked %d cells.", cellCount);      
00077         delete[] plan;
00078         
00079         if(foundFrontier)
00080         {
00081                 return EXPL_TARGET_SET;
00082         }else
00083         {
00084                 if(cellCount > 50)
00085                         return EXPL_FINISHED;
00086                 else
00087                         return EXPL_FAILED;
00088         }
00089 }


nav2d_exploration
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:17