00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_BASE_GOAL_
00038 #define OMPL_BASE_GOAL_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/SpaceInformation.h"
00042 #include "ompl/base/Path.h"
00043 #include "ompl/util/ClassForward.h"
00044 #include "ompl/base/GoalTypes.h"
00045 #include "ompl/util/Console.h"
00046 #include <iostream>
00047 #include <boost/noncopyable.hpp>
00048 #include <boost/concept_check.hpp>
00049 #include <vector>
00050
00051 namespace ompl
00052 {
00053 namespace base
00054 {
00056
00057 ClassForward(Goal);
00059
00064 struct PlannerSolution
00065 {
00067 PlannerSolution(const PathPtr &path, bool approximate = false, double difference = -1.0) :
00068 index_(-1), path_(path), length_(path->length()), approximate_(approximate), difference_(difference)
00069 {
00070 }
00071
00073 bool operator==(const PlannerSolution& p) const
00074 {
00075 return path_ == p.path_;
00076 }
00077
00079 bool operator<(const PlannerSolution &b) const
00080 {
00081 if (!approximate_ && b.approximate_)
00082 return true;
00083 if (approximate_ && !b.approximate_)
00084 return false;
00085 if (approximate_ && b.approximate_)
00086 return difference_ < b.difference_;
00087 return length_ < b.length_;
00088 }
00089
00091 int index_;
00092
00094 PathPtr path_;
00095
00097 double length_;
00098
00100 bool approximate_;
00101
00103 double difference_;
00104 };
00105
00107 class Goal : private boost::noncopyable
00108 {
00109 public:
00110
00112 Goal(const SpaceInformationPtr &si);
00113
00115 virtual ~Goal(void)
00116 {
00117 }
00118
00120 template<class T>
00121 T* as(void)
00122 {
00124 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Goal*>));
00125
00126 return static_cast<T*>(this);
00127 }
00128
00130 template<class T>
00131 const T* as(void) const
00132 {
00134 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Goal*>));
00135
00136 return static_cast<const T*>(this);
00137 }
00138
00140 GoalType getType(void) const
00141 {
00142 return type_;
00143 }
00144
00146 bool hasType(GoalType type) const
00147 {
00148 return (type_ & type) == type;
00149 }
00150
00152 const SpaceInformationPtr& getSpaceInformation(void) const
00153 {
00154 return si_;
00155 }
00156
00159 virtual bool isSatisfied(const State *st) const = 0;
00160
00172 virtual bool isSatisfied(const State *st, double *distance) const;
00173
00183 bool isSatisfied(const State *st, double pathLength, double *distance) const;
00184
00191 virtual bool isStartGoalPairValid(const State * , const State * ) const
00192 {
00193 return true;
00194 }
00195
00197 double getMaximumPathLength(void) const
00198 {
00199 return maximumPathLength_;
00200 }
00201
00207 void setMaximumPathLength(double maximumPathLength)
00208 {
00209 maximumPathLength_ = maximumPathLength;
00210 }
00211
00213 bool isPathLengthSatisfied(double pathLength) const
00214 {
00215 return pathLength <= maximumPathLength_;
00216 }
00217
00219 bool isAchieved(void) const;
00220
00224 bool isApproximate(void) const;
00225
00227 double getDifference(void) const;
00228
00232 PathPtr getSolutionPath(void) const;
00233
00238 void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0) const;
00239
00241 std::size_t getSolutionCount(void) const;
00242
00244 std::vector<PlannerSolution> getSolutions(void) const;
00245
00247 void clearSolutionPaths(void) const;
00248
00250 virtual void print(std::ostream &out = std::cout) const;
00251
00252 protected:
00253
00255 GoalType type_;
00256
00258 SpaceInformationPtr si_;
00259
00261 double maximumPathLength_;
00262
00264 msg::Interface msg_;
00265
00266 private:
00267
00269 ClassForward(PlannerSolutionSet);
00271
00273 PlannerSolutionSetPtr solutions_;
00274 };
00275
00276 }
00277 }
00278
00279 #endif