MinPosPlanner.cpp
Go to the documentation of this file.
1 // MinPos Exploration Planner
2 // Detailed description in:
3 // Bautin, A., & Simonin, O. (2012). MinPos : A Novel Frontier Allocation Algorithm for Multi-robot Exploration. ICIRA, 496–508.
4 
5 #include "MinPosPlanner.h"
6 #include <visualization_msgs/Marker.h>
7 
8 MinPosPlanner::MinPosPlanner()
9 {
10  ros::NodeHandle robotNode;
11  robotNode.param("robot_id", mRobotID, 1);
12 
13  ros::NodeHandle navigatorNode("~/");
14  navigatorNode.param("min_target_area_size", mMinTargetAreaSize, 10.0);
15  navigatorNode.param("visualize_frontiers", mVisualizeFrontiers, false);
16 
17  if(mVisualizeFrontiers)
18  {
19  mFrontierPublisher = navigatorNode.advertise<visualization_msgs::Marker>("frontiers", 1, true);
20  }
21 
22  mPlan = NULL;
23 }
24 
25 MinPosPlanner::~MinPosPlanner()
26 {
27  if(mPlan)
28  delete[] mPlan;
29 }
30 
31 typedef std::multimap<double,unsigned int> Queue;
32 typedef std::pair<double,unsigned int> Entry;
33 
34 int MinPosPlanner::findExplorationTarget(GridMap* map, unsigned int start, unsigned int &goal)
35 {
36  // Create some workspace for the wavefront algorithm
37  unsigned int mapSize = map->getSize();
38  if(mPlan)
39  delete[] mPlan;
40  mPlan = new double[mapSize];
41  for(unsigned int i = 0; i < mapSize; i++)
42  {
43  mPlan[i] = -1;
44  }
45 
46  mOffset[0] = -1; // left
47  mOffset[1] = 1; // right
48  mOffset[2] = -map->getWidth(); // up
49  mOffset[3] = map->getWidth(); // down
50  mOffset[4] = -map->getWidth() - 1;
51  mOffset[5] = -map->getWidth() + 1;
52  mOffset[6] = map->getWidth() - 1;
53  mOffset[7] = map->getWidth() + 1;
54 
55  // 1. Frontiers identification and clustering
56  // =========================================================================
57  mFrontiers.clear();
58  mFrontierCells = 0;
59 
60  // Initialize the queue with the robot position
61  Queue queue;
62  Entry startPoint(0.0, start);
63  queue.insert(startPoint);
64  mPlan[start] = 0;
65 
66  Queue::iterator next;
67  unsigned int x, y, index;
68  double linear = map->getResolution();
69  int cellCount = 0;
70 
71  // Search for frontiers with wavefront propagation
72  while(!queue.empty())
73  {
74  cellCount++;
75 
76  // Get the nearest cell from the queue
77  next = queue.begin();
78  double distance = next->first;
79  index = next->second;
80  queue.erase(next);
81 
82  // Now continue 1st level WPA
83  for(unsigned int it = 0; it < 4; it++)
84  {
85  unsigned int i = index + mOffset[it];
86  if(mPlan[i] == -1 && map->isFree(i))
87  {
88  // Check if it is a frontier cell
89  if(map->isFrontier(i))
90  {
91  findCluster(map, i);
92  }else
93  {
94  queue.insert(Entry(distance+linear, i));
95  }
96  mPlan[i] = distance+linear;
97  }
98  }
99  }
100 
101  ROS_DEBUG("[MinPos] Found %d frontier cells in %d frontiers.", mFrontierCells, (int)mFrontiers.size());
102  if(mFrontiers.size() == 0)
103  {
104  if(cellCount > 50)
105  {
106  return EXPL_FINISHED;
107  }else
108  {
109  ROS_WARN("[MinPos] No Frontiers found after checking %d cells!", cellCount);
110  return EXPL_FAILED;
111  }
112  }
113 
114  // Publish frontiers as marker for RVIZ
115  if(mVisualizeFrontiers)
116  {
117  visualization_msgs::Marker marker;
118  marker.header.frame_id = "/map";
119  marker.header.stamp = ros::Time();
120  marker.id = 0;
121  marker.type = visualization_msgs::Marker::CUBE_LIST;
122  marker.action = visualization_msgs::Marker::ADD;
123  marker.pose.position.x = map->getOriginX() + (map->getResolution() / 2);
124  marker.pose.position.y = map->getOriginY() + (map->getResolution() / 2);
125  marker.pose.position.z = map->getResolution() / 2;
126  marker.pose.orientation.x = 0.0;
127  marker.pose.orientation.y = 0.0;
128  marker.pose.orientation.z = 0.0;
129  marker.pose.orientation.w = 1.0;
130  marker.scale.x = map->getResolution();
131  marker.scale.y = map->getResolution();
132  marker.scale.z = map->getResolution();
133  marker.color.a = 0.5;
134  marker.color.r = 1.0;
135  marker.color.g = 1.0;
136  marker.color.b = 1.0;
137  marker.points.resize(mFrontierCells);
138  marker.colors.resize(mFrontierCells);
139 
140  unsigned int p = 0;
141  srand(1337);
142  for(unsigned int i = 0; i < mFrontiers.size(); i++)
143  {
144  char r = rand() % 256;
145  char g = rand() % 256;
146  char b = rand() % 256;
147  for(unsigned int j = 0; j < mFrontiers[i].size(); j++)
148  {
149  if(p < mFrontierCells)
150  {
151  if(!map->getCoordinates(x, y, mFrontiers[i][j]))
152  {
153  ROS_ERROR("[MinPos] getCoordinates failed!");
154  break;
155  }
156  marker.points[p].x = x * map->getResolution();
157  marker.points[p].y = y * map->getResolution();
158  marker.points[p].z = 0;
159 
160  marker.colors[p].r = r;
161  marker.colors[p].g = g;
162  marker.colors[p].b = b;
163  marker.colors[p].a = 0.5;
164  }else
165  {
166  ROS_ERROR("[MinPos] SecurityCheck failed! (Asked for %d / %d)", p, mFrontierCells);
167  }
168  p++;
169  }
170  }
171  mFrontierPublisher.publish(marker);
172  }
173 
174  // 2. Computation of the distances to frontiers
175  unsigned int bestRank = 9999;
176  unsigned int bestFrontier = 0;
177  double bestDistance = 9999;
178 
179  // Get positions of other robots
180  std::set<unsigned int> indices;
181  PoseList l = mRobotList.getRobots();
182  for(PoseList::iterator p = l.begin(); p != l.end(); p++)
183  {
184  if((int)p->first == mRobotID) continue;
185  unsigned int robot_x = (double)(p->second.x - map->getOriginX()) / map->getResolution();
186  unsigned int robot_y = (double)(p->second.y - map->getOriginY()) / map->getResolution();
187  unsigned int robot = 0;
188  if(map->getIndex(robot_x, robot_y, robot))
189  {
190  indices.insert(robot);
191  ROS_DEBUG("[MinPos] Inserted robot at index %d.", robot);
192  }else
193  {
194  ROS_WARN("[MinPos] Couldn't get index of robot %d!", p->first);
195  }
196  }
197  ROS_DEBUG("[MinPos] Using known positions of %d other robots.", (int)indices.size());
198 
199  for(unsigned int frontier = 0; frontier < mFrontiers.size(); frontier++)
200  {
201  // Reset the plan
202  double* plan = new double[mapSize];
203  for(unsigned int i = 0; i < mapSize; i++) plan[i] = -1;
204  Queue frontQueue;
205  unsigned int rank = 0;
206  double distanceToRobot = -1;
207 
208  // Add all cells of current frontier
209  for(unsigned int cell = 0; cell < mFrontiers[frontier].size(); cell++)
210  {
211  plan[mFrontiers[frontier][cell]] = 0;
212  frontQueue.insert(Entry(0.0, mFrontiers[frontier][cell]));
213  }
214 
215  // Start wavefront propagation
216  while(!frontQueue.empty())
217  {
218  // Get the nearest cell from the queue
219  Queue::iterator next = frontQueue.begin();
220  double distance = next->first;
221  unsigned int index = next->second;
222  frontQueue.erase(next);
223 
224  if(index == start) // Wavefront reached start point
225  {
226  distanceToRobot = distance;
227  break;
228  }
229  if(indices.find(index) != indices.end()) rank++;
230 
231  for(unsigned int it = 0; it < 4; it++)
232  {
233  int ind = index + mOffset[it];
234  if(ind >= 0 && ind < (int)map->getSize() && map->getData(ind) == 0 && plan[ind] == -1)
235  {
236  plan[ind] = distance + map->getResolution();
237  frontQueue.insert(Entry(distance + map->getResolution(), ind));
238  }
239  }
240  }
241  ROS_DEBUG("[MinPos] Frontier %d has rank %d and distance %.2f.", frontier, rank, distanceToRobot);
242  if(distanceToRobot > 0 && (rank < bestRank || (rank == bestRank && distanceToRobot < bestDistance)))
243  {
244  bestRank = rank;
245  bestFrontier = frontier;
246  bestDistance = distanceToRobot;
247  }
248 
249  delete[] plan;
250  if(bestRank == 0) break; // Frontiers are ordered by distance, so this will not improve anymore.
251  }
252 
253  // 3. Assignment to a frontier
254  ROS_DEBUG("[MinPos] Best frontier is %d.", bestFrontier);
255 
256  if(bestFrontier >= mFrontiers.size())
257  {
258  ROS_ERROR("[MinPos] Could not determine a best frontier. (Best: %d, Available: %d)", bestFrontier+1, (int)mFrontiers.size());
259  return EXPL_FAILED;
260  }
261 
262  // 4. Navigation towards the assigned frontiers for a fixed time period.
263  // - This is done in Navigator.
264  goal = mFrontiers.at(bestFrontier).at(0);
265  return EXPL_TARGET_SET;
266 }
267 
268 void MinPosPlanner::findCluster(GridMap* map, unsigned int startCell)
269 {
270  // Create a new frontier and expand it
271  Frontier front;
272  int frontNumber = -2 - mFrontiers.size();
273  int minAreaSize = mMinTargetAreaSize / (map->getResolution() * map->getResolution());
274 
275  // Initialize a new queue with the found frontier cell
276  Queue frontQueue;
277  frontQueue.insert(Entry(0.0, startCell));
278  bool isBoundary = false;
279 
280 // Queue unexplQueue;
281 // int areaSize = 0;
282 
283  while(!frontQueue.empty())
284  {
285  // Get the nearest cell from the queue
286  Queue::iterator next = frontQueue.begin();
287  double distance = next->first;
288  unsigned int index = next->second;
289  unsigned int x, y;
290  frontQueue.erase(next);
291 
292  // Check if it is a frontier cell
293  if(!map->isFrontier(index)) continue;
294 
295  // Add it to current frontier
296  front.push_back(index);
297  mFrontierCells++;
298 
299  // Add all adjacent cells to queue
300  for(unsigned int it = 0; it < 4; it++)
301  {
302  int i = index + mOffset[it];
303  if(map->isFree(i) && mPlan[i] == -1)
304  {
305  mPlan[i] = distance + map->getResolution();
306  frontQueue.insert(Entry(distance + map->getResolution(), i));
307  }
308  }
309 /*
310  // Calculate the size of the adjacent unknown region (as a kind of expected information gain)
311  unexplQueue.insert(Entry(0.0, index));
312  while(!unexplQueue.empty() && !isBoundary && areaSize <= minAreaSize)
313  {
314  // Get the nearest cell from the queue
315  Queue::iterator next2 = unexplQueue.begin();
316  double distance2 = next2->first;
317  unexplQueue.erase(next2);
318 
319  unsigned int index2 = next2->second;
320  unsigned int i2, x2, y2;
321  if(!map->getCoords(x2, y2, index2))
322  {
323  ROS_WARN("[MinPos] Unknown cell in queue was out of map!");
324  continue;
325  }
326 
327  std::vector<unsigned int> neighbors;
328  if(x2 > 0 && map->getIndex(x2-1,y2 ,i2)) neighbors.push_back(i2); else isBoundary = true;
329  if(x2 < map->getWidth()-1 && map->getIndex(x2+1,y2 ,i2)) neighbors.push_back(i2); else isBoundary = true;
330  if(y2 > 0 && map->getIndex(x2 ,y2-1,i2)) neighbors.push_back(i2); else isBoundary = true;
331  if(y2 < map->getHeight()-1 && map->getIndex(x2 ,y2+1,i2)) neighbors.push_back(i2); else isBoundary = true;
332 
333  // Add all adjacent cells to queue
334  for(unsigned int it2 = 0; it2 < neighbors.size(); it2++)
335  {
336  i2 = neighbors[it2];
337  if(map->getData(i2) == -1 && mPlan[i2] != frontNumber)
338  {
339  mPlan[i2] = frontNumber;
340  unexplQueue.insert(Entry(distance2 + map->getResolution(), i2));
341  areaSize++;
342  }
343  }
344  }
345 
346  }
347 
348  ROS_DEBUG("[MinPos] Size of unknown area: %d / Boundary: %d", areaSize, isBoundary);
349  if(isBoundary || areaSize >= minAreaSize)
350  mFrontiers.push_back(front);
351 */
352  }
353  mFrontiers.push_back(front);
354 }
GridMap::getResolution
double getResolution()
ros::NodeHandle
RobotList::getRobots
PoseList getRobots()
MinPosPlanner::mVisualizeFrontiers
bool mVisualizeFrontiers
Definition: MinPosPlanner.h:32
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
MinPosPlanner::mFrontierPublisher
ros::Publisher mFrontierPublisher
Definition: MinPosPlanner.h:22
Entry
std::pair< double, unsigned int > Entry
GridMap::getOriginX
double getOriginX()
MinPosPlanner::mOffset
unsigned int mOffset[8]
Definition: MinPosPlanner.h:34
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROS_WARN
#define ROS_WARN(...)
GridMap::getSize
unsigned int getSize()
MinPosPlanner::findCluster
void findCluster(GridMap *map, unsigned int startCell)
Definition: MinPosPlanner.cpp:268
MinPosPlanner::mFrontierCells
unsigned int mFrontierCells
Definition: MinPosPlanner.h:28
GridMap::getData
signed char getData(unsigned int index)
Queue
std::multimap< double, unsigned int > Queue
EXPL_FINISHED
#define EXPL_FINISHED
GridMap::isFree
bool isFree(unsigned int index)
distance
double distance(double x0, double y0, double x1, double y1)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
MinPosPlanner::Frontier
std::vector< unsigned int > Frontier
Definition: MinPosPlanner.h:15
EXPL_TARGET_SET
#define EXPL_TARGET_SET
MinPosPlanner::findExplorationTarget
int findExplorationTarget(GridMap *map, unsigned int start, unsigned int &goal)
Definition: MinPosPlanner.cpp:34
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
GridMap::getWidth
unsigned int getWidth()
PoseList
std::map< unsigned int, geometry_msgs::Pose2D > PoseList
EXPL_FAILED
#define EXPL_FAILED
MinPosPlanner::mFrontiers
FrontierList mFrontiers
Definition: MinPosPlanner.h:26
MinPosPlanner::mPlan
double * mPlan
Definition: MinPosPlanner.h:27
GridMap::isFrontier
bool isFrontier(unsigned int index)
Queue
std::multimap< double, unsigned int > Queue
Definition: MinPosPlanner.cpp:31
GridMap::getIndex
bool getIndex(unsigned int x, unsigned int y, unsigned int &i)
ROS_ERROR
#define ROS_ERROR(...)
MinPosPlanner::mRobotList
RobotList mRobotList
Definition: MinPosPlanner.h:25
MinPosPlanner::mMinTargetAreaSize
double mMinTargetAreaSize
Definition: MinPosPlanner.h:33
GridMap::getOriginY
double getOriginY()
Entry
std::pair< double, unsigned int > Entry
Definition: MinPosPlanner.cpp:32
GridMap::getCoordinates
bool getCoordinates(unsigned int &x, unsigned int &y, unsigned int i)
ROS_DEBUG
#define ROS_DEBUG(...)

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