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; }