00001 #ifndef INCLUDE_NAVLIB_PATHPLANNER_PATH 00002 #define INCLUDE_NAVLIB_PATHPLANNER_PATH 00003 00004 #include "geometry.h" 00005 #include <assert.h> 00006 #include <iostream> 00007 #include <sstream> 00008 #include <string> 00009 00010 using std::istream; 00011 using std::ostream; 00012 00013 namespace navlib 00014 { 00016 class PathPoint : public Pose 00017 { 00018 public: 00019 PathPoint(); 00020 PathPoint(double x0, double y0, double theta0, double c0); 00021 PathPoint(const Pose& p0, double c0); 00022 double c; 00023 00025 double getDistance(const PathPoint& wp) const; 00027 bool operator==(const PathPoint &wp) const; 00029 bool operator<(const PathPoint &wp) const; 00030 }; 00031 00032 istream& operator >>(istream& is, PathPoint& p); 00033 ostream& operator <<(ostream& os, const PathPoint& p); 00034 00036 class Path 00037 { 00038 public: 00039 Path(); 00040 virtual ~Path() {} 00044 virtual bool collides(const Shape* body, std::vector<Point> obstacles); 00046 virtual Path* copy() = 0; 00051 virtual PathPoint getPathPoint(double d) const = 0; 00053 virtual std::vector<PathPoint> getPathPoints() const; 00055 PathPoint getInitPathPoint() const; 00057 PathPoint getFinalPathPoint() const; 00059 virtual double getLength() const = 0; 00061 virtual bool isReverse(double d) const; 00063 virtual void read(istream& is); 00065 virtual void write(ostream& os) const; 00066 00067 protected: 00068 PathPoint _wp0; 00069 }; 00070 00071 istream& operator >>(istream& is, Path& path); 00072 ostream& operator <<(ostream& os, const Path& path); 00073 00075 class PathChain : public Path 00076 { 00077 public: 00078 PathChain(); 00079 virtual ~PathChain(); 00080 bool collides(const Shape* body, std::vector<Point> obstacles); 00081 double getLength() const; 00082 PathPoint getPathPoint(double d) const; 00083 std::vector<PathPoint> getPathPoints() const; 00086 bool append(Path* T); 00087 Path* copy(); 00090 bool insert(Path* T); 00091 bool isReverse(double d) const; 00092 00093 private: 00094 std::vector<Path*> _pathList; 00095 double _length; 00096 }; 00097 00100 class ReversePath : public Path 00101 { 00102 public: 00103 ReversePath(Path* path); 00104 virtual ~ReversePath(); 00105 Path* copy(); 00106 PathPoint getPathPoint(double d) const; 00107 std::vector<PathPoint> getPathPoints() const; 00108 double getLength() const; 00109 bool isReverse(double d) const; 00110 00111 private: 00112 Path* _path; 00113 double _length; 00114 }; 00115 } 00116 00117 #endif 00118