MultiWavefrontPlanner.cpp
Go to the documentation of this file.
00001 #include "MultiWavefrontPlanner.h"
00002 #include <geometry_msgs/Pose2D.h>
00003 #include <nav_msgs/GridCells.h>
00004 
00005 typedef std::multimap<double,unsigned int> Queue;
00006 typedef std::pair<double,unsigned int> Entry;
00007 
00008 using namespace ros;
00009 
00010 MultiWavefrontPlanner::MultiWavefrontPlanner()
00011 {
00012         NodeHandle robotNode;
00013         robotNode.param("robot_id", mRobotID, 1);
00014         robotNode.param("map_frame", mMapFrame, std::string("map"));
00015         
00016         tf::TransformListener tf;
00017         std::string tfPrefix = tf.getTFPrefix();
00018         mMapFrame = tf::resolve(tfPrefix, mMapFrame);
00019         
00020         NodeHandle navigatorNode("~/");
00021         mWaitForOthers = false;
00022         
00023         // Initialize Publisher/Subscriber
00024         mWavefrontPublisher = navigatorNode.advertise<nav_msgs::GridCells>("wave", 1);
00025         mOtherWavefrontPublisher = navigatorNode.advertise<nav_msgs::GridCells>("others", 1);
00026 }
00027 
00028 MultiWavefrontPlanner::~MultiWavefrontPlanner()
00029 {
00030         
00031 }
00032 
00033 int MultiWavefrontPlanner::findExplorationTarget(GridMap* map, unsigned int start, unsigned int &goal)
00034 {
00035         // Initialize a search map
00036         unsigned int mapSize = map->getSize();
00037         int* searchMap = new int[mapSize];
00038         for(unsigned int i = 0; i < mapSize; i++)
00039         {
00040                 searchMap[i] = 0;
00041         }
00042         
00043         // Initialize the queue with the robot position
00044         Queue queue;
00045         Entry startPoint(0.0, start);
00046         queue.insert(startPoint);
00047         searchMap[start] = mRobotID;
00048         
00049         // A second queue for cells occupied by other robots
00050         Queue buffer;
00051         
00052         // Insert position of other robots
00053         PoseList l = mRobotList.getRobots();
00054         for(PoseList::iterator p = l.begin(); p != l.end(); p++)
00055         {
00056                 if((int)p->first == mRobotID) continue;
00057                 unsigned int robot_x =  (double)(p->second.x - map->getOriginX()) / map->getResolution();
00058                 unsigned int robot_y =  (double)(p->second.y - map->getOriginY()) / map->getResolution();
00059                 unsigned int robot = 0;
00060                 if(map->getIndex(robot_x, robot_y, robot))
00061                 {
00062                         queue.insert(Entry(0.0, robot));
00063                         searchMap[robot] = p->first;
00064                 }else
00065                 {
00066                         ROS_WARN("Couldn't place robot %d in search map!", p->first);
00067                 }
00068         }
00069 
00070         Queue::iterator next;
00071         double linear = map->getResolution();
00072         bool foundFrontier = false;
00073         bool hasFrontiers = false;
00074         int cellCount = 0;
00075         
00076         std::vector<std::pair<double, double> > points;
00077         std::vector<std::pair<double, double> > others;
00078         
00079         // Do full search with weightless Dijkstra-Algorithm
00080         while(!foundFrontier && !queue.empty())
00081         {
00082                 cellCount++;
00083                 // Get the nearest cell from the queue
00084                 next = queue.begin();
00085                 double distance = next->first;
00086                 unsigned int index = next->second;
00087                 queue.erase(next);
00088                 int currentBot = searchMap[index];
00089                 
00090                 // Add all adjacent cells
00091                 if(map->isFrontier(index))
00092                 {
00093                         hasFrontiers = true;
00094                         if(currentBot == mRobotID)
00095                         {
00096                                 foundFrontier = true;
00097                                 goal = index;
00098                         }
00099                 }else
00100                 {
00101                         std::vector<unsigned int> neighbors = map->getFreeNeighbors(index);
00102                         for(unsigned int i = 0; i < neighbors.size(); i++)
00103                         {
00104                                 unsigned int x,y;
00105                                 if(!map->getCoordinates(x,y,neighbors[i])) continue;
00106                                 if(searchMap[neighbors[i]] == 0)
00107                                 {
00108                                         queue.insert(Entry(distance+linear, neighbors[i]));
00109                                         searchMap[neighbors[i]] = currentBot;
00110                                         if(currentBot == mRobotID)
00111                                         {
00112                                                 points.push_back(std::pair<double, double>(((x+0.5) * map->getResolution()) + map->getOriginX(), ((y+0.5) * map->getResolution()) + map->getOriginY()));
00113                                         }else
00114                                         {
00115                                                 others.push_back(std::pair<double, double>(((x+0.5) * map->getResolution()) + map->getOriginX(), ((y+0.5) * map->getResolution()) + map->getOriginY()));
00116                                         }
00117                                 }else if(searchMap[neighbors[i]] != mRobotID && currentBot == mRobotID)
00118                                 {
00119                                         // We don't continue the dijkstra now, but keep the point for possible later use
00120                                         buffer.insert(Entry(distance+linear, neighbors[i]));
00121                                 }
00122                         }                               
00123                 }
00124         }
00125 
00126         // If not supposed to wait, now continue the wavefront into other robots area
00127         if(!mWaitForOthers)
00128         {
00129                 while(!foundFrontier && !buffer.empty())
00130                 {
00131                         cellCount++;
00132                         // Get the nearest cell from the buffer
00133                         next = buffer.begin();
00134                         double distance = next->first;
00135                         unsigned int index = next->second;
00136                         buffer.erase(next);
00137                         
00138                         // Add all adjacent cells
00139                         if(map->isFrontier(index))
00140                         {
00141                                 foundFrontier = true;
00142                                 goal = index;
00143                         }else
00144                         {
00145                                 std::vector<unsigned int> neighbors = map->getFreeNeighbors(index);
00146                                 for(unsigned int i = 0; i < neighbors.size(); i++)
00147                                 {
00148                                         if(map->isFree(neighbors[i]) && searchMap[neighbors[i]] != mRobotID)
00149                                         {
00150                                                 unsigned int x,y;
00151                                                 if(!map->getCoordinates(x,y,neighbors[i])) continue;
00152                                                 buffer.insert(Entry(distance+linear, neighbors[i]));
00153                                                 searchMap[neighbors[i]] = mRobotID;
00154                                                 points.push_back(std::pair<double, double>(((x+0.5) * map->getResolution()) + map->getOriginX(), ((y+0.5) * map->getResolution()) + map->getOriginY()));
00155                                         }
00156                                 }
00157                         }
00158                 }
00159         }
00160 
00161         delete[] searchMap;
00162         ROS_DEBUG("Checked %d cells.", cellCount);
00163         
00164         // GridCells for debugging in RVIZ
00165         nav_msgs::GridCells wave_msg;
00166         wave_msg.header.frame_id = mMapFrame.c_str();
00167         wave_msg.header.stamp = Time::now();
00168         wave_msg.cell_width = map->getResolution();
00169         wave_msg.cell_height = map->getResolution();
00170         wave_msg.cells.resize(points.size());
00171         for(unsigned int i = 0; i < points.size(); i++)
00172         {
00173                 wave_msg.cells[i].x = points[i].first;
00174                 wave_msg.cells[i].y = points[i].second;
00175                 wave_msg.cells[i].z = 0.0;
00176         }
00177         mWavefrontPublisher.publish(wave_msg);
00178         
00179         wave_msg.cells.resize(others.size());
00180         for(unsigned int i = 0; i < others.size(); i++)
00181         {
00182                 wave_msg.cells[i].x = others[i].first;
00183                 wave_msg.cells[i].y = others[i].second;
00184                 wave_msg.cells[i].z = 0.0;
00185         }
00186         mOtherWavefrontPublisher.publish(wave_msg);
00187         
00188         if(foundFrontier)
00189         {
00190                 return EXPL_TARGET_SET;
00191         }else if(hasFrontiers)
00192         {
00193                 if(buffer.size() > 0)
00194                         return EXPL_WAITING;
00195                 else
00196                 {
00197                         ROS_WARN("[MultiWave] Frontiers present, but not reachable.");
00198                         return EXPL_FAILED;
00199                 }
00200         }else
00201         {
00202                 if(cellCount > 50)
00203                         return EXPL_FINISHED;
00204                 else
00205                 {
00206                         ROS_WARN("[MultiWave] No Frontiers present, but robot seems locked.");
00207                         return EXPL_FAILED;
00208                 }
00209         }
00210 }


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