Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PathPlanner.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_PathPlanner_h_
00003 #define INCLUDED_PathPlanner_h_
00004 
00005 #include <map>
00006 #include <iostream>
00007 #include <queue>
00008 #include <map>
00009 #include <bitset>
00010 #include "DualCoding.h"
00011 
00012 namespace DualCoding {
00013 
00014 // plans path given start and goal locations, landmarks, obstacles and area allowed to be in.
00015 // path is a set of waypoints which consist of position, heading and landmark(s).
00016 // path is actually computed from goal to start, which I guess could have been other way around
00017 class PathPlanner {
00018 public:
00019   PathPlanner(coordinate_t xMax, coordinate_t xMin, coordinate_t yMax, coordinate_t yMin);
00020   virtual ~PathPlanner() { 
00021     delete [] unreached; 
00022     delete [] costMap; 
00023   }
00024 
00025   //{ data structures used to represent the map and path
00026   struct direction {
00027     std::bitset<2> bitVal; // 00=N, 01=W, 10=S, 11=E
00028     direction (unsigned int i) : bitVal(i) {}
00029     operator unsigned int() const { return (unsigned int) bitVal.to_ulong(); }
00030     enum directions { N=0,W=1,S=2,E=3 };// dir;
00031     direction cw() const { direction cwd(bitVal.to_ulong()+1); return cwd; }
00032     direction ccw() const { direction ccwd(bitVal.to_ulong()-1); return ccwd; }
00033   };
00034   struct location {
00035     unsigned int pos;
00036     direction dir;
00037     location()
00038       : pos(0), dir(direction::N) {}
00039     location(unsigned int _pos, direction _dir)
00040       : pos(_pos), dir(_dir) {}
00041     location(const location& l)
00042       : pos(l.pos), dir(l.dir) {}
00043     bool operator==(const location& l) const { return pos==l.pos && dir==l.dir; }
00044   };
00045   struct state { // structure representing each node 
00046     location loc;
00047     unsigned int cost;
00048     std::pair<int,int> lms;
00049     state* dst; // optimal transition to be found
00050     state(location _loc, int lm1, int lm2)
00051       : loc(_loc), cost(0), lms(lm1, lm2), dst(NULL) {} //, links() {}
00052     state(location _loc, std::pair<int,int> _lms)
00053       : loc(_loc), cost(0), lms(_lms), dst(NULL) {} //, links() {}
00054     state(const state& s)
00055       : loc(s.loc), cost(s.cost), lms(s.lms), dst(&(*s.dst)) {} //, links(s.links) {}
00056     state& operator=(const state& s) 
00057     { if (this!=&s) { loc = s.loc; cost = s.cost; lms = s.lms; dst = s.dst; } return *this; }
00058   };
00059   struct edge { // structure representing transition b/w two states
00060     state *from, *to;
00061     unsigned int cost; // cost of transition
00062     edge(state& _from, state& _to, unsigned int _cost) : from(&_from), to(&_to), cost(_cost) {}
00063     edge(const edge& l) : from(&(*l.from)), to(&(*l.to)), cost(l.cost) {}
00064     edge& operator=(const edge& l) 
00065     { if (this!=&l) { from = &(*l.from); to = &(*l.to); cost = l.cost; } return *this; }
00066   };
00067   //}
00068 
00069 protected:
00070   std::queue<state> allStates; // all possible states
00071   std::vector<state*> *unreached; // collection of states for which optimal action is not determined yet
00072   // priority queue containing edges evaluated so far in order of increasing cost, core of this algorithm
00073   struct lessCost { bool operator()(const edge& e1, const edge& e2) const { return e1.cost > e2.cost; }};
00074   std::priority_queue<edge,std::vector<edge>,lessCost> reached;
00075   static const int size=300; // length of dog
00076   unsigned int numX, numY;
00077   float minX, minY, dX, dY;
00078   location start, goal;
00079 public:
00080   unsigned int maxDistance;
00081   enum Cost { transF, transB, transY, tranB, rotate, noLM, oneLM };
00082   std::map <Cost, unsigned int> costs; // all costs must be positive for this algorithm to work
00083   Point startPt, goalPt;
00084   AngTwoPi startOrientation, goalOrientation;
00085   
00086 protected:
00087   unsigned int *costMap; // measure of how close you are to obstacles
00088   // maps b/w shape id and location/color info
00089   std::map<unsigned int, PointData> landmarks; 
00090   // maps b/w shape ids and their score(distinguishability from other lms) 
00091   std::map<unsigned int, unsigned int> lmCosts; 
00092   std::vector<std::pair<Point, unsigned int> > obstacles; //<! position of obstacles and their relative costs
00093 
00094 public:
00095   void findPath(); // finds path from goal to start using Dijkstra's algorithm
00096   void addLandmark(const ShapeRoot& lm) { //adds landmark
00097     landmarks.insert(std::make_pair<unsigned int,PointData>
00098          (lm->getId(),PointData(VRmixin::worldShS,Point(lm->getCentroid()))));
00099   }
00100   void addLandmarkAsObstacle(const ShapeRoot& lm, unsigned int cost) { // adds landmark also as obstacle
00101     addLandmark(lm);
00102     obstacles.push_back(std::pair<Point,unsigned int>(lm->getCentroid(),cost));
00103   }
00104 
00105 protected:
00106   bool isLMVisible(location loc, const Point& lm); // finds if landmark is visible from the location
00107   void initialize(); // defines all possible states (vertices)
00108   void fillState(unsigned int low, unsigned int high);
00109   void findLinks(state& s); // find all possible links for given state and store them inside the state
00110   std::vector<std::pair<int,int> > findLMs(location loc);
00111   state* thereIs(unsigned int pos, direction dir, std::pair<int,int> lms);
00112   void computeLandmarkCosts();
00113   Point findWorldCoords(unsigned int pos);
00114   std::string toString(const PathPlanner::state &s);
00115 
00116 private:
00117   PathPlanner& operator=(const PathPlanner&);
00118   PathPlanner(const PathPlanner&);
00119 
00120 };
00121   std::ostream& operator<<(std::ostream& out, const PathPlanner::direction& d);
00122 
00123 } // namespace
00124 
00125 #endif

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