1 /********************************************************************* 2 * Software License Agreement (BSD License) 3 * 4 * Copyright (c) 2008, Willow Garage, Inc. 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 11 * * Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * * Redistributions in binary form must reproduce the above 14 * copyright notice, this list of conditions and the following 15 * disclaimer in the documentation and/or other materials provided 16 * with the distribution. 17 * * Neither the name of the Willow Garage nor the names of its 18 * contributors may be used to endorse or promote products derived 19 * from this software without specific prior written permission. 20 * 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 * POSSIBILITY OF SUCH DAMAGE. 33 *********************************************************************/ 34 35 /* Author: Ioan Sucan */ 36 37 #ifndef OMPL_BASE_GOAL_ 38 #define OMPL_BASE_GOAL_ 39 40 #include "ompl/base/State.h" 41 #include "ompl/base/SpaceInformation.h" 42 #include "ompl/util/ClassForward.h" 43 #include "ompl/base/GoalTypes.h" 44 #include "ompl/util/Console.h" 45 #include <iostream> 46 #include <boost/concept_check.hpp> 47 #include <vector> 48 49 namespace ompl 50 { 51 namespace base 52 { 53 /// @cond IGNORE 54 /** \brief Forward declaration of ompl::base::Goal */ 55 OMPL_CLASS_FORWARD(Goal); 56 /// @endcond 57 58 /** \class ompl::base::GoalPtr 59 \brief A shared pointer wrapper for ompl::base::Goal */ 60 61 /** \brief Abstract definition of goals.*/ 62 class Goal 63 { 64 public: 65 // non-copyable 66 Goal(const Goal &) = delete; 67 Goal &operator=(const Goal &) = delete; 68 69 /** \brief Constructor. The goal must always know the space information it is part of */ 70 Goal(SpaceInformationPtr si); 71 72 /** \brief Destructor.*/ 73 virtual ~Goal() = default; 74 75 /** \brief Cast this instance to a desired type. */ 76 template <class T> as()77 T *as() 78 { 79 /** \brief Make sure the type we are casting to is indeed a goal */ 80 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Goal *>)); 81 82 return static_cast<T *>(this); 83 } 84 85 /** \brief Cast this instance to a desired type. */ 86 template <class T> as()87 const T *as() const 88 { 89 /** \brief Make sure the type we are casting to is indeed a goal */ 90 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Goal *>)); 91 92 return static_cast<const T *>(this); 93 } 94 95 /** \brief Return the goal type */ getType()96 GoalType getType() const 97 { 98 return type_; 99 } 100 101 /** \brief Check if this goal can be cast to a particular goal type */ hasType(GoalType type)102 bool hasType(GoalType type) const 103 { 104 return (type_ & type) == type; 105 } 106 107 /** \brief Get the space information this goal is for */ getSpaceInformation()108 const SpaceInformationPtr &getSpaceInformation() const 109 { 110 return si_; 111 } 112 113 /** \brief Return true if the state satisfies the goal 114 * constraints. */ 115 virtual bool isSatisfied(const State *st) const = 0; 116 117 /** \brief Return true if the state satisfies the goal 118 * constraints and compute the distance between the state 119 * given as argument and the goal (even if the goal is 120 * not satisfied). This distance can be an 121 * approximation. It can even be set to a constant, if 122 * such a computation is not possible. 123 * \param st the state to check for validity 124 * \param distance location at which distance to goal will be stored 125 * \note The default implementation sets the distance to a constant. 126 * \note If this function returns true, 127 * isStartGoalPairValid() need not be called. */ 128 virtual bool isSatisfied(const State *st, double *distance) const; 129 130 /** \brief Since there can be multiple starting states 131 (and multiple goal states) it is possible certain 132 pairs are not to be allowed. By default we however 133 assume all such pairs are allowed. Note: if this 134 function returns true, isSatisfied() need not be 135 called. */ isStartGoalPairValid(const State *,const State *)136 virtual bool isStartGoalPairValid(const State * /* start */, const State * /* goal */) const 137 { 138 return true; 139 } 140 141 /** \brief Print information about the goal */ 142 virtual void print(std::ostream &out = std::cout) const; 143 144 protected: 145 /** \brief Goal type */ 146 GoalType type_; 147 148 /** \brief The space information for this goal */ 149 SpaceInformationPtr si_; 150 }; 151 } 152 } 153 154 #endif 155