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/Goal.h"
00038
00039 #include <algorithm>
00040 #include <limits>
00041
00042 #include <boost/thread/mutex.hpp>
00043
00045 namespace ompl
00046 {
00047 namespace base
00048 {
00049
00050 class Goal::PlannerSolutionSet
00051 {
00052 public:
00053
00054 PlannerSolutionSet(void)
00055 {
00056 }
00057
00058 void add(const PlannerSolution &s)
00059 {
00060 boost::mutex::scoped_lock slock(lock_);
00061 int index = solutions_.size();
00062 solutions_.push_back(s);
00063 solutions_.back().index_ = index;
00064 std::sort(solutions_.begin(), solutions_.end());
00065 }
00066
00067 void clear(void)
00068 {
00069 boost::mutex::scoped_lock slock(lock_);
00070 solutions_.clear();
00071 }
00072
00073 std::vector<PlannerSolution> getSolutions(void)
00074 {
00075 boost::mutex::scoped_lock slock(lock_);
00076 std::vector<PlannerSolution> copy = solutions_;
00077 return copy;
00078 }
00079
00080 bool isApproximate(void)
00081 {
00082 boost::mutex::scoped_lock slock(lock_);
00083 bool result = false;
00084 if (!solutions_.empty())
00085 result = solutions_[0].approximate_;
00086 return result;
00087 }
00088
00089 double getDifference(void)
00090 {
00091 boost::mutex::scoped_lock slock(lock_);
00092 double diff = -1.0;
00093 if (!solutions_.empty())
00094 diff = solutions_[0].difference_;
00095 return diff;
00096 }
00097
00098 PathPtr getTopSolution(void)
00099 {
00100 boost::mutex::scoped_lock slock(lock_);
00101 PathPtr copy;
00102 if (!solutions_.empty())
00103 copy = solutions_[0].path_;
00104 return copy;
00105 }
00106
00107 std::size_t getSolutionCount(void)
00108 {
00109 boost::mutex::scoped_lock slock(lock_);
00110 std::size_t result = solutions_.size();
00111 return result;
00112 }
00113
00114 private:
00115
00116 std::vector<PlannerSolution> solutions_;
00117 boost::mutex lock_;
00118 };
00119 }
00120 }
00122
00123 ompl::base::Goal::Goal(const SpaceInformationPtr &si) :
00124 type_(GOAL_ANY), si_(si), maximumPathLength_(std::numeric_limits<double>::infinity()),
00125 msg_("Goal"), solutions_(new PlannerSolutionSet())
00126 {
00127 }
00128
00129 bool ompl::base::Goal::isAchieved(void) const
00130 {
00131 return solutions_->getSolutionCount() > 0;
00132 }
00133
00134 std::size_t ompl::base::Goal::getSolutionCount(void) const
00135 {
00136 return solutions_->getSolutionCount();
00137 }
00138
00139 ompl::base::PathPtr ompl::base::Goal::getSolutionPath(void) const
00140 {
00141 return solutions_->getTopSolution();
00142 }
00143
00144 void ompl::base::Goal::addSolutionPath(const PathPtr &path, bool approximate, double difference) const
00145 {
00146 if (approximate)
00147 msg_.warn("Adding approximate solution");
00148 solutions_->add(PlannerSolution(path, approximate, difference));
00149 }
00150
00151 bool ompl::base::Goal::isApproximate(void) const
00152 {
00153 return solutions_->isApproximate();
00154 }
00155
00156 double ompl::base::Goal::getDifference(void) const
00157 {
00158 return solutions_->getDifference();
00159 }
00160
00161 std::vector<ompl::base::PlannerSolution> ompl::base::Goal::getSolutions(void) const
00162 {
00163 return solutions_->getSolutions();
00164 }
00165
00166 void ompl::base::Goal::clearSolutionPaths(void) const
00167 {
00168 solutions_->clear();
00169 }
00170
00171 bool ompl::base::Goal::isSatisfied(const State *st, double *distance) const
00172 {
00173 if (distance != NULL)
00174 *distance = std::numeric_limits<double>::max();
00175 return isSatisfied(st);
00176 }
00177
00178 bool ompl::base::Goal::isSatisfied(const State *st, double pathLength, double *distance) const
00179 {
00180 if (pathLength > maximumPathLength_)
00181 {
00182 if (distance != NULL)
00183 isSatisfied(st, distance);
00184 return false;
00185 }
00186 else
00187 return isSatisfied(st, distance);
00188 }
00189
00190 void ompl::base::Goal::print(std::ostream &out) const
00191 {
00192 out << "Goal memory address " << this << std::endl;
00193 out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl;
00194 }