NearestFrontierPlanner.cpp
Go to the documentation of this file.
2 
3 typedef std::multimap<double,unsigned int> Queue;
4 typedef std::pair<double,unsigned int> Entry;
5 
7 {
8 
9 }
10 
12 {
13 
14 }
15 
16 int NearestFrontierPlanner::findExplorationTarget(GridMap* map, unsigned int start, unsigned int &goal)
17 {
18  // Create some workspace for the wavefront algorithm
19  unsigned int mapSize = map->getSize();
20  double* plan = new double[mapSize];
21  for(unsigned int i = 0; i < mapSize; i++)
22  {
23  plan[i] = -1;
24  }
25 
26  // Initialize the queue with the robot position
27  Queue queue;
28  Entry startPoint(0.0, start);
29  queue.insert(startPoint);
30  plan[start] = 0;
31 
32  Queue::iterator next;
33  double distance;
34  double linear = map->getResolution();
35  bool foundFrontier = false;
36  int cellCount = 0;
37 
38  // Do full search with weightless Dijkstra-Algorithm
39  while(!queue.empty())
40  {
41  cellCount++;
42  // Get the nearest cell from the queue
43  next = queue.begin();
44  distance = next->first;
45  unsigned int index = next->second;
46  queue.erase(next);
47 
48  // Add all adjacent cells
49  if(map->isFrontier(index))
50  {
51  // We reached the border of the map, which is unexplored terrain as well:
52  foundFrontier = true;
53  goal = index;
54  break;
55  }else
56  {
57  unsigned int ind[4];
58 
59  ind[0] = index - 1; // left
60  ind[1] = index + 1; // right
61  ind[2] = index - map->getWidth(); // up
62  ind[3] = index + map->getWidth(); // down
63 
64  for(unsigned int it = 0; it < 4; it++)
65  {
66  unsigned int i = ind[it];
67  if(map->isFree(i) && plan[i] == -1)
68  {
69  queue.insert(Entry(distance+linear, i));
70  plan[i] = distance+linear;
71  }
72  }
73  }
74  }
75 
76  ROS_DEBUG("Checked %d cells.", cellCount);
77  delete[] plan;
78 
79  if(foundFrontier)
80  {
81  return EXPL_TARGET_SET;
82  }else
83  {
84  if(cellCount > 50)
85  return EXPL_FINISHED;
86  else
87  return EXPL_FAILED;
88  }
89 }
double getResolution()
std::multimap< double, unsigned int > Queue
std::pair< double, unsigned int > Entry
unsigned int getSize()
std::multimap< double, unsigned int > Queue
#define EXPL_FINISHED
bool isFree(unsigned int index)
double distance(double x0, double y0, double x1, double y1)
int findExplorationTarget(GridMap *map, unsigned int start, unsigned int &goal)
#define EXPL_TARGET_SET
unsigned int getWidth()
std::pair< double, unsigned int > Entry
#define EXPL_FAILED
bool isFrontier(unsigned int index)
#define ROS_DEBUG(...)


nav2d_exploration
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:48