All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
Goal.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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 * /* start */, const State * /* goal */) 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
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends