00001
00002 #include "PathPlanner.h"
00003 #include "Shared/ProjectInterface.h"
00004 #include <iostream>
00005 #include <ctime>
00006 #include <vector>
00007 #include <queue>
00008 #include <map>
00009 #include <math.h>
00010
00011 using namespace std;
00012
00013 namespace DualCoding {
00014
00015 using ProjectInterface::defSegmentedColorGenerator;
00016
00017 PathPlanner::PathPlanner(coordinate_t xMax, coordinate_t xMin, coordinate_t yMax, coordinate_t yMin)
00018 : allStates(), unreached(NULL), reached(),
00019 numX((unsigned int) ((xMax-xMin)/size)+1), numY((unsigned int) ((yMax-yMin)/size)+1),
00020 minX(xMin), minY(yMin), dX((xMax-xMin)/numX), dY((yMax-yMin)/numY),
00021 start(), goal(), maxDistance(800), costs(),
00022 startPt(), goalPt(),startOrientation(), goalOrientation(),
00023 costMap(NULL), landmarks(), lmCosts(), obstacles()
00024 {
00025 unreached = new vector<state*>[numX*numY*4];
00026 costMap = new unsigned int [numX*numY];
00027
00028 costs[transF] = 1;
00029 costs[transB] = 2;
00030 costs[transY] = 4;
00031 costs[rotate] = 3;
00032 costs[noLM] = 20;
00033 costs[oneLM] = 10;
00034 }
00035
00036 void PathPlanner::findPath() {
00037 initialize();
00038 while (!reached.empty()) {
00039 edge top = reached.top();
00040 if (top.from != NULL && top.from->dst == NULL) {
00041 top.from->cost = top.cost;
00042 top.from->dst = top.to;
00043 vector<state*>& lms = unreached[top.from->loc.pos*4 + top.from->loc.dir];
00044 for (vector<state*>::iterator it = lms.begin();
00045 it != lms.end(); it++)
00046 if ((*it)->lms == top.from->lms) {
00047 lms.erase(it);
00048 break;
00049 }
00050 if (top.from->loc==start) {
00051 cout << "best path:\n";
00052 state* s = top.from;
00053 while(s) {
00054 cout << toString(*s) << endl;
00055 s = s->dst;
00056 }
00057 cout << endl;
00058 break;
00059 }
00060 else
00061 findLinks(*top.from);
00062 }
00063 reached.pop();
00064 }
00065 cout << "done: " << reached.empty() << endl;
00066 }
00067
00068 void PathPlanner::findLinks(state& s) {
00069 unsigned int newCostMotion = s.cost+lmCosts[s.lms.first]+lmCosts[s.lms.second]+1;
00070 if (s.lms.first < 0)
00071 if (s.lms.second < 0)
00072 newCostMotion += costs[noLM];
00073 else
00074 newCostMotion += costs[oneLM];
00075 {
00076 if (s.loc.pos%numY+1 < numY)
00077 if (state* it = thereIs(s.loc.pos+1,s.loc.dir,s.lms))
00078 if (s.loc.dir==direction::E)
00079 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transB]));
00080 else if (s.loc.dir == direction::W)
00081 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transF]));
00082 else
00083 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transY]));
00084 if (s.loc.pos%numY > 0)
00085 if (state* it = thereIs(s.loc.pos-1,s.loc.dir,s.lms))
00086 if (s.loc.dir==direction::E)
00087 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transF]));
00088 else if (s.loc.dir == direction::W)
00089 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transB]));
00090 else
00091 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transY]));
00092 if (s.loc.pos < numY*(numX-1))
00093 if (state* it = thereIs(s.loc.pos+numY,s.loc.dir,s.lms))
00094 if (s.loc.dir==direction::N)
00095 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transF]));
00096 else if (s.loc.dir == direction::S)
00097 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transB]));
00098 else
00099 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transY]));
00100 if (s.loc.pos >= numY)
00101 if (state* it = thereIs(s.loc.pos-numY,s.loc.dir,s.lms))
00102 if (s.loc.dir==direction::N)
00103 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transB]));
00104 else if (s.loc.dir == direction::S)
00105 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transF]));
00106 else
00107 reached.push(edge(*it,s,newCostMotion+costMap[s.loc.pos]+costs[transY]));
00108 }
00109 {
00110 const int newRotateCost = newCostMotion+costs[rotate];
00111 if (state* it = thereIs(s.loc.pos,s.loc.dir.cw(),s.lms))
00112 reached.push(edge(*it,s,newRotateCost+costMap[s.loc.pos]));
00113 if (state* it = thereIs(s.loc.pos,s.loc.dir.ccw(),s.lms))
00114 reached.push(edge(*it,s,newRotateCost+costMap[s.loc.pos]));
00115 }
00116 {
00117 vector<pair<int,int> > lms = findLMs(s.loc);
00118 for (vector<pair<int,int> >::const_iterator it = lms.begin();
00119 it != lms.end(); it++)
00120 if (*it != s.lms)
00121 reached.push(edge(*(thereIs(s.loc.pos,s.loc.dir,*it)),s,s.cost+1));
00122 }
00123 }
00124
00125
00126 Point PathPlanner::findWorldCoords(unsigned int pos) {
00127 return Point (minX+dX*(pos/numY),minY+dY*(pos%numY),0);
00128 }
00129
00130
00131 void PathPlanner::computeLandmarkCosts() {
00132 map<unsigned int, float> distSum;
00133 map<unsigned int, vector<unsigned int> > lmsSortedByColor;
00134 for (map<unsigned int, PointData>::const_iterator it1 = landmarks.begin();
00135 it1 != landmarks.end(); it1++) {
00136 map<unsigned int, PointData>::const_iterator it2 = it1;
00137 it2++;
00138 for (; it2 != landmarks.end(); it2++)
00139 if (it1->second.getColor() == it2->second.getColor()) {
00140 float dist = it1->second.getCentroid().xyDistanceFrom(it2->second);
00141 float cost = 10000/(dist*dist);
00142 distSum[it1->first] += cost;
00143 distSum[it2->first] += cost;
00144 }
00145 lmsSortedByColor[defSegmentedColorGenerator->getColorIndex(it1->second.getColor())].push_back(it1->first);
00146 }
00147 for (map<unsigned int, vector<unsigned int> >::const_iterator col_it = lmsSortedByColor.begin();
00148 col_it != lmsSortedByColor.end(); col_it++)
00149 for (vector<unsigned int>::const_iterator lm_it = col_it->second.begin();
00150 lm_it != col_it->second.end(); lm_it++)
00151 lmCosts[*lm_it] = (unsigned int) distSum[*lm_it];
00152 }
00153
00154 void PathPlanner::initialize() {
00155
00156 float minStartDist = findWorldCoords(0).distanceFrom(startPt);
00157 float minGoalDist = findWorldCoords(0).distanceFrom(goalPt);
00158 unsigned int startIdx=0, goalIdx=0;
00159 for (unsigned int pos = 1; pos < numX*numY; pos++) {
00160 float startDist = findWorldCoords(pos).distanceFrom(startPt);
00161 float goalDist = findWorldCoords(pos).distanceFrom(goalPt);
00162 if (startDist < minStartDist) {
00163 minStartDist = startDist;
00164 startIdx = pos;
00165 }
00166 if (goalDist < minGoalDist) {
00167 minGoalDist = goalDist;
00168 goalIdx = pos;
00169 }
00170 }
00171 start.pos = startIdx;
00172 goal.pos = goalIdx;
00173
00174 if (startOrientation > M_PI/4) {
00175 if (startOrientation < M_PI*3/4)
00176 start.dir = direction::W;
00177 else if (startOrientation < M_PI*5/4)
00178 start.dir = direction::S;
00179 else if (startOrientation < M_PI*7/4)
00180 start.dir = direction::E;
00181 }
00182 else
00183 start.dir = direction::N;
00184
00185 if (goalOrientation > M_PI/4) {
00186 if (goalOrientation < M_PI*3/4)
00187 goal.dir = direction::W;
00188 else if (goalOrientation < M_PI*5/4)
00189 goal.dir = direction::S;
00190 else if (goalOrientation < M_PI*7/4)
00191 goal.dir = direction::E;
00192 }
00193 else
00194 goal.dir = direction::N;
00195
00196
00197
00198 for (unsigned int pos = 0; pos < numX*numY; pos++) {
00199 costMap[pos] = 0;
00200 for (vector<pair<Point, unsigned int> >::const_iterator it = obstacles.begin();
00201 it != obstacles.end(); it++) {
00202 Point vec = findWorldCoords(pos)-it->first;
00203 float distSq = (vec.coordX()*vec.coordX() + vec.coordY()*vec.coordY())/10000;
00204 costMap[pos] += (unsigned int) ((distSq > 0) ? it->second/distSq : -1U);
00205 }
00206 }
00207 computeLandmarkCosts();
00208 cout << "costs:\n";
00209 cout << " costs[transF]: " << costs[transF] << endl;
00210 cout << " costs[transB]: " << costs[transB] << endl;
00211 cout << " costs[transY]: " << costs[transY] << endl;
00212 cout << " costs[rotate]: " << costs[rotate] << endl;
00213 cout << " costs[noLM]: " << costs[noLM] << endl;
00214 cout << " costs[oneLM]: " << costs[oneLM] << endl;
00215
00216 cout << "landmarks:\n";
00217 for (map<unsigned int, PointData>::const_iterator it = landmarks.begin();
00218 it != landmarks.end(); it++)
00219 cout<< "lm id " << it->first << " at " << it->second
00220
00221 << ", cost=" << lmCosts[it->first] << endl;
00222
00223 cout << "obstacles:\n";
00224 for (vector<pair<Point, unsigned int> >::const_iterator it = obstacles.begin();
00225 it != obstacles.end(); it++)
00226 cout << " " << it->first << ": " << it->second << endl;
00227
00228
00229 cout << "costMap:\n";
00230 for (int j = numX-1; j >= 0; j--) {
00231 for (int i = numY-1; i >= 0; i--)
00232 if (i+j*numY == start.pos)
00233 cout << " S";
00234 else if (i+j*numY == goal.pos)
00235 cout << " G";
00236 else
00237 cout << " " << costMap[i+j*numY];
00238 cout << endl;
00239 }
00240
00241 while (!reached.empty())
00242 reached.pop();
00243 for (unsigned int i = 0; i < numX*numY*4; i++)
00244 unreached[i].clear();
00245 while (!allStates.empty())
00246 allStates.pop();
00247
00248
00249 vector<state*> goals;
00250 fillState(0,goal.pos);
00251 for (unsigned int i = 0; i < 4; i++) {
00252 location loc (goal.pos,i);
00253 vector<pair<int,int> > lms = findLMs(loc);
00254 for (vector<pair<int,int> >::const_iterator it = lms.begin();
00255 it < lms.end(); it++) {
00256 allStates.push(state(loc,*it));
00257 if (i == goal.dir)
00258 goals.push_back(&allStates.back());
00259 else
00260 unreached[loc.pos*4 + loc.dir].push_back(&(allStates.back()));
00261 }
00262 }
00263 fillState(goal.pos+1,numX*numY);
00264
00265 cout << "start state:\n";
00266 cout << " " << findWorldCoords(start.pos) << endl;
00267
00268
00269
00270 cout << "goal state(s):\n";
00271 for (vector<state*>::iterator it = goals.begin();
00272 it != goals.end(); it++) {
00273 (*it)->cost = 0;
00274 findLinks(**it);
00275 cout << " " << toString(**it) << " => " << findWorldCoords((*it)->loc.pos) << endl;
00276 }
00277 }
00278
00279 void PathPlanner::fillState(unsigned int low, unsigned int high) {
00280 for (unsigned int pos = low; pos < high; pos++) {
00281 for (unsigned int i = 0; i < 4; i++) {
00282 location loc(pos,i);
00283 vector<pair<int,int> > lms = findLMs(loc);
00284 for (vector<pair<int,int> >::const_iterator it = lms.begin();
00285 it < lms.end(); it++) {
00286 allStates.push(state(loc,*it));
00287 unreached[loc.pos*4 + loc.dir].push_back(&(allStates.back()));
00288 }
00289 }
00290 }
00291 }
00292
00293 PathPlanner::state* PathPlanner::thereIs(unsigned int pos, PathPlanner::direction dir, std::pair<int,int> lms) {
00294
00295 const vector<state*>& states = unreached[pos*4 + (unsigned int) dir];
00296 for (vector<state*>::const_iterator it = states.begin();
00297 it < states.end(); it++)
00298 if ((*it)->lms == lms)
00299 return *it;
00300 return NULL;
00301 }
00302
00303
00304 std::vector<std::pair<int,int> > PathPlanner::findLMs(location loc) {
00305 vector<int> lms;
00306 lms.push_back(-1);
00307 for (map<unsigned int, PointData>::const_iterator it = landmarks.begin();
00308 it != landmarks.end(); it++)
00309 if (isLMVisible(loc,it->second)) lms.push_back(it->first);
00310 vector<pair<int,int> > lmPairs;
00311 lmPairs.push_back(pair<int,int>(-1,-1));
00312 for(vector<int>::const_iterator it1 = lms.begin();
00313 it1 != lms.end(); it1++)
00314 for(vector<int>::const_iterator it2 = it1+1;
00315 it2 != lms.end(); it2++)
00316 lmPairs.push_back(pair<int,int>(*it1,*it2));
00317 return lmPairs;
00318 }
00319
00320 bool PathPlanner::isLMVisible(location l, const Point& lm) {
00321 Point agent = findWorldCoords(l.pos);
00322 float dx = lm.coordX()-agent.coordX();
00323 float dy = lm.coordY()-agent.coordY();
00324 float distance = sqrt(dx*dx + dy*dy);
00325
00326 if (distance > maxDistance || distance < 200) return false;
00327 AngTwoPi angle = atan2(dy,dx);
00328
00329 switch (l.dir) {
00330 case direction::N: return (angle < M_PI/3 || angle > M_PI*5/3);
00331 case direction::W: return (angle > M_PI/6 && angle < M_PI*5/6);
00332 case direction::S: return (angle > M_PI*2/3 && angle < M_PI*4/3);
00333 case direction::E: return (angle > M_PI*7/6 && angle < M_PI*11/6);
00334 };
00335 return false;
00336 }
00337
00338 std::string PathPlanner::toString(const PathPlanner::state &s) {
00339 stringstream ss;
00340 ss << "[(" << (s.loc.pos/numY) << ',' << (s.loc.pos%numY) << ','
00341 << s.loc.dir << "), (" << s.lms.first << ',' << s.lms.second << "), " << s.cost << ']';
00342 return ss.str();
00343 }
00344
00345 std::ostream& operator<<(std::ostream& out, const PathPlanner::direction& d) {
00346 switch (d) {
00347 case PathPlanner::direction::N: out << "N"; break;
00348 case PathPlanner::direction::W: out << "W"; break;
00349 case PathPlanner::direction::S: out << "S"; break;
00350 default: out << "E"; break;
00351 };
00352 return out;
00353 }
00354
00355
00356 }