MinPosPlanner.cpp
Go to the documentation of this file.
00001 // MinPos Exploration Planner
00002 // Detailed description in:
00003 // Bautin, A., & Simonin, O. (2012). MinPos : A Novel Frontier Allocation Algorithm for Multi-robot Exploration. ICIRA, 496–508.
00004 
00005 #include "MinPosPlanner.h"
00006 #include <visualization_msgs/Marker.h>
00007 
00008 MinPosPlanner::MinPosPlanner()
00009 {
00010         ros::NodeHandle robotNode;
00011         robotNode.param("robot_id", mRobotID, 1);
00012         
00013         ros::NodeHandle navigatorNode("~/");
00014         navigatorNode.param("min_target_area_size", mMinTargetAreaSize, 10.0);
00015         navigatorNode.param("visualize_frontiers", mVisualizeFrontiers, false);
00016         
00017         if(mVisualizeFrontiers)
00018         {
00019                 mFrontierPublisher = navigatorNode.advertise<visualization_msgs::Marker>("frontiers", 1, true);
00020         }
00021         
00022         mPlan = NULL;
00023 }
00024 
00025 MinPosPlanner::~MinPosPlanner()
00026 {
00027         if(mPlan)
00028                 delete[] mPlan;
00029 }
00030 
00031 typedef std::multimap<double,unsigned int> Queue;
00032 typedef std::pair<double,unsigned int> Entry;
00033 
00034 int MinPosPlanner::findExplorationTarget(GridMap* map, unsigned int start, unsigned int &goal)
00035 {
00036         // Create some workspace for the wavefront algorithm
00037         unsigned int mapSize = map->getSize();
00038         if(mPlan)
00039                 delete[] mPlan;
00040         mPlan = new double[mapSize];
00041         for(unsigned int i = 0; i < mapSize; i++)
00042         {
00043                 mPlan[i] = -1;
00044         }
00045 
00046         mOffset[0] = -1;                                        // left
00047         mOffset[1] =  1;                                        // right
00048         mOffset[2] = -map->getWidth();  // up
00049         mOffset[3] =  map->getWidth();  // down
00050         mOffset[4] = -map->getWidth() - 1;
00051         mOffset[5] = -map->getWidth() + 1;
00052         mOffset[6] =  map->getWidth() - 1;
00053         mOffset[7] =  map->getWidth() + 1;
00054         
00055         // 1. Frontiers identification and clustering
00056         // =========================================================================
00057         mFrontiers.clear();
00058         mFrontierCells = 0;
00059 
00060         // Initialize the queue with the robot position
00061         Queue queue;
00062         Entry startPoint(0.0, start);
00063         queue.insert(startPoint);
00064         mPlan[start] = 0;
00065         
00066         Queue::iterator next;
00067         unsigned int x, y, index;
00068         double linear = map->getResolution();
00069         int cellCount = 0;
00070 
00071         // Search for frontiers with wavefront propagation
00072         while(!queue.empty())
00073         {
00074                 cellCount++;
00075                 
00076                 // Get the nearest cell from the queue
00077                 next = queue.begin();
00078                 double distance = next->first;
00079                 index = next->second;
00080                 queue.erase(next);
00081                 
00082                 // Now continue 1st level WPA
00083                 for(unsigned int it = 0; it < 4; it++)
00084                 {
00085                         unsigned int i = index + mOffset[it];
00086                         if(mPlan[i] == -1 && map->isFree(i))
00087                         {
00088                                 // Check if it is a frontier cell
00089                                 if(map->isFrontier(i))
00090                                 {
00091                                         findCluster(map, i);
00092                                 }else
00093                                 {
00094                                         queue.insert(Entry(distance+linear, i));
00095                                 }
00096                                 mPlan[i] = distance+linear;
00097                         }
00098                 }
00099         }
00100 
00101         ROS_DEBUG("[MinPos] Found %d frontier cells in %d frontiers.", mFrontierCells, (int)mFrontiers.size());
00102         if(mFrontiers.size() == 0)
00103         {
00104                 if(cellCount > 50)
00105                 {
00106                         return EXPL_FINISHED;
00107                 }else
00108                 {
00109                         ROS_WARN("[MinPos] No Frontiers found after checking %d cells!", cellCount);
00110                         return EXPL_FAILED;
00111                 }
00112         }
00113         
00114         // Publish frontiers as marker for RVIZ
00115         if(mVisualizeFrontiers)
00116         {
00117                 visualization_msgs::Marker marker;
00118                 marker.header.frame_id = "/map";
00119                 marker.header.stamp = ros::Time();
00120                 marker.id = 0;
00121                 marker.type = visualization_msgs::Marker::CUBE_LIST;
00122                 marker.action = visualization_msgs::Marker::ADD;
00123                 marker.pose.position.x = map->getOriginX() + (map->getResolution() / 2);
00124                 marker.pose.position.y = map->getOriginY() + (map->getResolution() / 2);
00125                 marker.pose.position.z = map->getResolution() / 2;
00126                 marker.pose.orientation.x = 0.0;
00127                 marker.pose.orientation.y = 0.0;
00128                 marker.pose.orientation.z = 0.0;
00129                 marker.pose.orientation.w = 1.0;
00130                 marker.scale.x = map->getResolution();
00131                 marker.scale.y = map->getResolution();
00132                 marker.scale.z = map->getResolution();
00133                 marker.color.a = 0.5;
00134                 marker.color.r = 1.0;
00135                 marker.color.g = 1.0;
00136                 marker.color.b = 1.0;
00137                 marker.points.resize(mFrontierCells);
00138                 marker.colors.resize(mFrontierCells);
00139                 
00140                 unsigned int p = 0;
00141                 srand(1337);
00142                 for(unsigned int i = 0; i < mFrontiers.size(); i++)
00143                 {
00144                         char r = rand() % 256;
00145                         char g = rand() % 256;
00146                         char b = rand() % 256;
00147                         for(unsigned int j = 0; j < mFrontiers[i].size(); j++)
00148                         {
00149                                 if(p < mFrontierCells)
00150                                 {
00151                                         if(!map->getCoordinates(x, y, mFrontiers[i][j]))
00152                                         {
00153                                                 ROS_ERROR("[MinPos] getCoordinates failed!");
00154                                                 break;
00155                                         }
00156                                         marker.points[p].x = x * map->getResolution();
00157                                         marker.points[p].y = y * map->getResolution();
00158                                         marker.points[p].z = 0;
00159                                         
00160                                         marker.colors[p].r = r;
00161                                         marker.colors[p].g = g;
00162                                         marker.colors[p].b = b;
00163                                         marker.colors[p].a = 0.5;
00164                                 }else
00165                                 {
00166                                         ROS_ERROR("[MinPos] SecurityCheck failed! (Asked for %d / %d)", p, mFrontierCells);
00167                                 }
00168                                 p++;
00169                         }
00170                 }
00171                 mFrontierPublisher.publish(marker);
00172         }
00173         
00174         // 2. Computation of the distances to frontiers
00175         unsigned int bestRank = 9999;
00176         unsigned int bestFrontier = 0;
00177         double bestDistance = 9999;
00178         
00179         // Get positions of other robots
00180         std::set<unsigned int> indices;
00181         PoseList l = mRobotList.getRobots();
00182         for(PoseList::iterator p = l.begin(); p != l.end(); p++)
00183         {
00184                 if((int)p->first == mRobotID) continue;
00185                 unsigned int robot_x =  (double)(p->second.x - map->getOriginX()) / map->getResolution();
00186                 unsigned int robot_y =  (double)(p->second.y - map->getOriginY()) / map->getResolution();
00187                 unsigned int robot = 0;
00188                 if(map->getIndex(robot_x, robot_y, robot))
00189                 {
00190                         indices.insert(robot);
00191                         ROS_DEBUG("[MinPos] Inserted robot at index %d.", robot);
00192                 }else
00193                 {
00194                         ROS_WARN("[MinPos] Couldn't get index of robot %d!", p->first);
00195                 }
00196         }
00197         ROS_DEBUG("[MinPos] Using known positions of %d other robots.", (int)indices.size());
00198         
00199         for(unsigned int frontier = 0; frontier < mFrontiers.size(); frontier++)
00200         {
00201                 // Reset the plan
00202                 double* plan = new double[mapSize];
00203                 for(unsigned int i = 0; i < mapSize; i++) plan[i] = -1;
00204                 Queue frontQueue;
00205                 unsigned int rank = 0;
00206                 double distanceToRobot = -1;
00207                 
00208                 // Add all cells of current frontier
00209                 for(unsigned int cell = 0; cell < mFrontiers[frontier].size(); cell++)
00210                 {
00211                         plan[mFrontiers[frontier][cell]] = 0;
00212                         frontQueue.insert(Entry(0.0, mFrontiers[frontier][cell]));
00213                 }
00214                 
00215                 // Start wavefront propagation
00216                 while(!frontQueue.empty())
00217                 {
00218                         // Get the nearest cell from the queue
00219                         Queue::iterator next = frontQueue.begin();
00220                         double distance = next->first;
00221                         unsigned int index = next->second;
00222                         frontQueue.erase(next);
00223                         
00224                         if(index == start) // Wavefront reached start point
00225                         {
00226                                 distanceToRobot = distance;
00227                                 break;
00228                         }
00229                         if(indices.find(index) != indices.end()) rank++; 
00230                         
00231                         for(unsigned int it = 0; it < 4; it++)
00232                         {
00233                                 int ind = index + mOffset[it];
00234                                 if(ind >= 0 && ind < (int)map->getSize() && map->getData(ind) == 0 && plan[ind] == -1)
00235                                 {
00236                                         plan[ind] = distance + map->getResolution();
00237                                         frontQueue.insert(Entry(distance + map->getResolution(), ind));
00238                                 }
00239                         }
00240                 }
00241                 ROS_DEBUG("[MinPos] Frontier %d has rank %d and distance %.2f.", frontier, rank, distanceToRobot);
00242                 if(distanceToRobot > 0 && (rank < bestRank || (rank == bestRank && distanceToRobot < bestDistance)))
00243                 {
00244                         bestRank = rank;
00245                         bestFrontier = frontier;
00246                         bestDistance = distanceToRobot;
00247                 }
00248                 
00249                 delete[] plan;
00250                 if(bestRank == 0) break; // Frontiers are ordered by distance, so this will not improve anymore.
00251         }
00252         
00253         // 3. Assignment to a frontier
00254         ROS_DEBUG("[MinPos] Best frontier is %d.", bestFrontier);
00255         
00256         if(bestFrontier >= mFrontiers.size())
00257         {
00258                 ROS_ERROR("[MinPos] Could not determine a best frontier. (Best: %d, Available: %d)", bestFrontier+1, (int)mFrontiers.size());
00259                 return EXPL_FAILED;
00260         }
00261         
00262         // 4. Navigation towards the assigned frontiers for a fixed time period.
00263         // - This is done in Navigator.
00264         goal = mFrontiers.at(bestFrontier).at(0);
00265         return EXPL_TARGET_SET;
00266 }
00267 
00268 void MinPosPlanner::findCluster(GridMap* map, unsigned int startCell)
00269 {
00270         // Create a new frontier and expand it
00271         Frontier front;
00272         int frontNumber = -2 - mFrontiers.size();
00273         int minAreaSize = mMinTargetAreaSize / (map->getResolution() * map->getResolution());
00274         
00275         // Initialize a new queue with the found frontier cell
00276         Queue frontQueue;
00277         frontQueue.insert(Entry(0.0, startCell));
00278         bool isBoundary = false;
00279         
00280 //      Queue unexplQueue;
00281 //      int areaSize = 0;
00282         
00283         while(!frontQueue.empty())
00284         {
00285                 // Get the nearest cell from the queue
00286                 Queue::iterator next = frontQueue.begin();
00287                 double distance = next->first;
00288                 unsigned int index = next->second;
00289                 unsigned int x, y;
00290                 frontQueue.erase(next);
00291                 
00292                 // Check if it is a frontier cell
00293                 if(!map->isFrontier(index)) continue;
00294                 
00295                 // Add it to current frontier
00296                 front.push_back(index);
00297                 mFrontierCells++;
00298                 
00299                 // Add all adjacent cells to queue
00300                 for(unsigned int it = 0; it < 4; it++)
00301                 {
00302                         int i = index + mOffset[it];
00303                         if(map->isFree(i) && mPlan[i] == -1)
00304                         {
00305                                 mPlan[i] = distance + map->getResolution();
00306                                 frontQueue.insert(Entry(distance + map->getResolution(), i));
00307                         }
00308                 }
00309 /*
00310                 // Calculate the size of the adjacent unknown region (as a kind of expected information gain)
00311                 unexplQueue.insert(Entry(0.0, index));
00312                 while(!unexplQueue.empty() && !isBoundary && areaSize <= minAreaSize)
00313                 {
00314                         // Get the nearest cell from the queue
00315                         Queue::iterator next2 = unexplQueue.begin();
00316                         double distance2 = next2->first;
00317                         unexplQueue.erase(next2);
00318                         
00319                         unsigned int index2 = next2->second;
00320                         unsigned int i2, x2, y2;
00321                         if(!map->getCoords(x2, y2, index2))
00322                         {
00323                                 ROS_WARN("[MinPos] Unknown cell in queue was out of map!");
00324                                 continue;
00325                         }
00326                         
00327                         std::vector<unsigned int> neighbors;
00328                         if(x2 > 0                  && map->getIndex(x2-1,y2  ,i2)) neighbors.push_back(i2); else isBoundary = true;
00329                         if(x2 < map->getWidth()-1  && map->getIndex(x2+1,y2  ,i2)) neighbors.push_back(i2); else isBoundary = true;
00330                         if(y2 > 0                  && map->getIndex(x2  ,y2-1,i2)) neighbors.push_back(i2); else isBoundary = true;
00331                         if(y2 < map->getHeight()-1 && map->getIndex(x2  ,y2+1,i2)) neighbors.push_back(i2); else isBoundary = true;
00332                         
00333                         // Add all adjacent cells to queue
00334                         for(unsigned int it2 = 0; it2 < neighbors.size(); it2++)
00335                         {
00336                                 i2 = neighbors[it2];
00337                                 if(map->getData(i2) == -1 && mPlan[i2] != frontNumber)
00338                                 {
00339                                         mPlan[i2] = frontNumber;
00340                                         unexplQueue.insert(Entry(distance2 + map->getResolution(), i2));
00341                                         areaSize++;
00342                                 }
00343                         }
00344                 }
00345 
00346         }
00347 
00348         ROS_DEBUG("[MinPos] Size of unknown area: %d / Boundary: %d", areaSize, isBoundary);
00349         if(isBoundary || areaSize >= minAreaSize)
00350                 mFrontiers.push_back(front);
00351 */
00352         }
00353         mFrontiers.push_back(front);
00354 }


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