FrontierList Planner::getFrontiers(GridMap* map, GridPoint start)
{
// Initialization
mFrontierCellCount = 0;
mFrontierCount = 0;
GridMap plan = GridMap(map->getWidth(), map->getHeight());
FrontierList result;
// Initialize the queue with the robot position
Queue queue;
queue.insert(Entry(0, start));
plan.setData(start, VISIBLE);
// Do full search with weightless Dijkstra-Algorithm
while(!queue.empty())
{
// Get the nearest cell from the queue
Queue::iterator next = queue.begin();
int distance = next->first;
GridPoint point = next->second;
queue.erase(next);
// Add neighbors
bool isFrontier = false;
PointList neighbors = getNeighbors(point, false);
char c = 0;
for(PointList::const_iterator cell = neighbors.begin(); cell < neighbors.end(); cell++)
{
if(map->getData(*cell,c) && c == UNKNOWN)
{
plan.setData(*cell, OBSTACLE);
isFrontier = true;
break;
}
if(map->getData(*cell, c) && c == VISIBLE &&
plan.getData(*cell, c) && c == UNKNOWN)
{
queue.insert(Entry(distance+1, *cell));
plan.setData(*cell, VISIBLE);
}
}
if(isFrontier)
{
result.push_back(getFrontier(map, &plan, point));
}
}
// Set result message and return the point list
if(result.size() > 0)
{
mStatus = SUCCESS;
sprintf(mStatusMessage, "Found %d frontiers with %d frontier cells.", mFrontierCount, mFrontierCellCount);
}else
{
mStatus = NO_GOAL;
sprintf(mStatusMessage, "No reachable frontiers found.");
}
return result;
}
PointList Planner::getFrontierCells(GridMap* map, GridPoint start, bool stopAtFirst)
{
// Initialization
mFrontierCellCount = 0;
GridMap plan = GridMap(map->getWidth(), map->getHeight());
PointList result;
// Initialize the queue with the robot position
Queue queue;
queue.insert(Entry(0, start));
plan.setData(start, 0);
// Do full search with weightless Dijkstra-Algorithm
while(!queue.empty())
{
// Get the nearest cell from the queue
Queue::iterator next = queue.begin();
int distance = next->first;
GridPoint point = next->second;
queue.erase(next);
bool foundFrontier = false;
// Add all adjacent cells
PointList neighbors = getNeighbors(point);
for(PointList::const_iterator cell = neighbors.begin(); cell < neighbors.end(); cell++)
{
char c = 0;
if(map->getData(*cell, c) && c == UNKNOWN)
{
foundFrontier = true;
continue;
}
if(map->getData(*cell, c) && c == VISIBLE && plan.getData(*cell, c) && c == UNKNOWN)
{
queue.insert(Entry(distance+1, *cell));
plan.setData(*cell,0);
}
}
if(foundFrontier)
{
GridPoint frontier = point;
frontier.distance = distance;
result.push_back(frontier);
mFrontierCellCount++;
if(stopAtFirst) break;
}
}
// Set result message and return the point list
if(result.size() > 0)
{
mStatus = SUCCESS;
sprintf(mStatusMessage, "Found %d reachable frontier cells.", (int)result.size());
}else
{
mStatus = NO_GOAL;
sprintf(mStatusMessage, "No reachable frontier cells found.");
}
return result;
}
