37 #include "ompl/base/goals/GoalStates.h"
38 #include "ompl/base/SpaceInformation.h"
39 #include "ompl/util/Exception.h"
40 #include <boost/lexical_cast.hpp>
43 ompl::base::GoalStates::~GoalStates(
void)
54 void ompl::base::GoalStates::freeMemory(
void)
56 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
57 si_->freeState(states_[i]);
62 double dist = std::numeric_limits<double>::infinity();
63 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
65 double d = si_->distance(st, states_[i]);
74 out << states_.size() <<
" goal states, threshold = " << threshold_ <<
", memory address = " <<
this << std::endl;
75 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
77 si_->printState(states_[i], out);
85 throw Exception(
"There are no goals to sample");
86 si_->copyState(st, states_[samplePosition_]);
87 samplePosition_ = (samplePosition_ + 1) % states_.size();
92 return states_.size();
97 states_.push_back(si_->cloneState(st));
107 if (index >= states_.size())
108 throw Exception(
"Index " + boost::lexical_cast<std::string>(index) +
" out of range. Only " +
109 boost::lexical_cast<std::string>(states_.size()) +
" states are available");
110 return states_[index];
115 return states_.size();
120 return !states_.empty();
virtual bool hasStates(void) const
Check if there are any states in this goal region.
Definition of a scoped state.
virtual double distanceGoal(const State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
virtual void print(std::ostream &out=std::cout) const
Print information about the goal data structure to a stream.
virtual unsigned int maxSampleCount(void) const
Return the maximum number of samples that can be asked for before repeating.
virtual std::size_t getStateCount(void) const
Return the number of valid goal states.
virtual const State * getState(unsigned int index) const
Return a pointer to the indexth state in the state list.
virtual void addState(const State *st)
Add a goal state.
virtual void sampleGoal(State *st) const
Sample a state in the goal region.
StateType * get(void)
Returns a pointer to the contained state.
Definition of an abstract state.
The exception type for ompl.
virtual void clear(void)
Clear all goal states.