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 #include "ompl/base/goals/GoalStates.h"
00038 #include "ompl/base/SpaceInformation.h"
00039 #include "ompl/util/Exception.h"
00040 #include <boost/lexical_cast.hpp>
00041 #include <limits>
00042
00043 ompl::base::GoalStates::~GoalStates(void)
00044 {
00045 freeMemory();
00046 }
00047
00048 void ompl::base::GoalStates::clear(void)
00049 {
00050 freeMemory();
00051 states_.clear();
00052 }
00053
00054 void ompl::base::GoalStates::freeMemory(void)
00055 {
00056 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00057 si_->freeState(states_[i]);
00058 }
00059
00060 double ompl::base::GoalStates::distanceGoal(const State *st) const
00061 {
00062 double dist = std::numeric_limits<double>::infinity();
00063 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00064 {
00065 double d = si_->distance(st, states_[i]);
00066 if (d < dist)
00067 dist = d;
00068 }
00069 return dist;
00070 }
00071
00072 void ompl::base::GoalStates::print(std::ostream &out) const
00073 {
00074 out << states_.size() << " goal states, threshold = " << threshold_ << ", memory address = " << this << std::endl;
00075 for (unsigned int i = 0 ; i < states_.size() ; ++i)
00076 {
00077 si_->printState(states_[i], out);
00078 out << std::endl;
00079 }
00080 }
00081
00082 void ompl::base::GoalStates::sampleGoal(base::State *st) const
00083 {
00084 if (states_.empty())
00085 throw Exception("There are no goals to sample");
00086 si_->copyState(st, states_[samplePosition_]);
00087 samplePosition_ = (samplePosition_ + 1) % states_.size();
00088 }
00089
00090 unsigned int ompl::base::GoalStates::maxSampleCount(void) const
00091 {
00092 return states_.size();
00093 }
00094
00095 void ompl::base::GoalStates::addState(const State* st)
00096 {
00097 states_.push_back(si_->cloneState(st));
00098 }
00099
00100 void ompl::base::GoalStates::addState(const ScopedState<> &st)
00101 {
00102 addState(st.get());
00103 }
00104
00105 const ompl::base::State* ompl::base::GoalStates::getState(unsigned int index) const
00106 {
00107 if (index >= states_.size())
00108 throw Exception("Index " + boost::lexical_cast<std::string>(index) + " out of range. Only " +
00109 boost::lexical_cast<std::string>(states_.size()) + " states are available");
00110 return states_[index];
00111 }
00112
00113 std::size_t ompl::base::GoalStates::getStateCount(void) const
00114 {
00115 return states_.size();
00116 }
00117
00118 bool ompl::base::GoalStates::hasStates(void) const
00119 {
00120 return !states_.empty();
00121 }