Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PathPlanner.cc

Go to the documentation of this file.
00001 //-*-c++-*-
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   // default costs; these can be modefied anytime before calling findPath()
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) { // best action not determined yet for this state
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   { //translate
00076     if (s.loc.pos%numY+1 < numY) // go west
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) // go east
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)) // go north
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) // go south
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   { //rotate
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   { //switch landmarks
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; // maps id of landmark against sum of distance with other landmarks of same color
00133   map<unsigned int, vector<unsigned int> > lmsSortedByColor; // maps color index against vector of idlandmarks' id having the color
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; // cannot write it2 = it1+1?
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   // set start and goal
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   // initialize cost map
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; // cm^2
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       //  << ", color index=" << defSegmentedColorGenerator->getColorIndex(it->second.getColor())
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   // print map
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   // find all possible vertices (states)
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   //  for (vector<
00268   //   cout << " is lm 1 visible ? " << isLMVisible(start,pt) << endl;
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   //  cout << "thereIs: " << pos << ',' << dir << ',' << (pos+dir) << endl;
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); // no landmark is also an option
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   //  cout << "distance: " << distance << endl;
00326   if (distance > maxDistance || distance < 200) return false; // too far or too close
00327   AngTwoPi angle = atan2(dy,dx);
00328   //  cout << "angle: " << angle << endl;
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 }

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4