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/util/ClassForward.h"
00043 #include "ompl/base/GoalTypes.h"
00044 #include "ompl/util/Console.h"
00045 #include <iostream>
00046 #include <boost/noncopyable.hpp>
00047 #include <boost/concept_check.hpp>
00048 #include <vector>
00049
00050 namespace ompl
00051 {
00052 namespace base
00053 {
00055
00056 ClassForward(Goal);
00058
00063 class Goal : private boost::noncopyable
00064 {
00065 public:
00066
00068 Goal(const SpaceInformationPtr &si);
00069
00071 virtual ~Goal(void)
00072 {
00073 }
00074
00076 template<class T>
00077 T* as(void)
00078 {
00080 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Goal*>));
00081
00082 return static_cast<T*>(this);
00083 }
00084
00086 template<class T>
00087 const T* as(void) const
00088 {
00090 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Goal*>));
00091
00092 return static_cast<const T*>(this);
00093 }
00094
00096 GoalType getType(void) const
00097 {
00098 return type_;
00099 }
00100
00102 bool hasType(GoalType type) const
00103 {
00104 return (type_ & type) == type;
00105 }
00106
00108 const SpaceInformationPtr& getSpaceInformation(void) const
00109 {
00110 return si_;
00111 }
00112
00115 virtual bool isSatisfied(const State *st) const = 0;
00116
00128 virtual bool isSatisfied(const State *st, double *distance) const;
00129
00139 bool isSatisfied(const State *st, double pathLength, double *distance) const;
00140
00147 virtual bool isStartGoalPairValid(const State * , const State * ) const
00148 {
00149 return true;
00150 }
00151
00153 double getMaximumPathLength(void) const
00154 {
00155 return maximumPathLength_;
00156 }
00157
00163 void setMaximumPathLength(double maximumPathLength)
00164 {
00165 maximumPathLength_ = maximumPathLength;
00166 }
00167
00169 bool isPathLengthSatisfied(double pathLength) const
00170 {
00171 return pathLength <= maximumPathLength_;
00172 }
00173
00175 virtual void print(std::ostream &out = std::cout) const;
00176
00177 protected:
00178
00180 GoalType type_;
00181
00183 SpaceInformationPtr si_;
00184
00186 double maximumPathLength_;
00187 };
00188
00189 }
00190 }
00191
00192 #endif