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();