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/Planner.h"
00038 #include "ompl/util/Exception.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/base/GoalLazySamples.h"
00041 #include <boost/thread.hpp>
00042
00043 ompl::base::Planner::Planner(const SpaceInformationPtr &si, const std::string &name) :
00044 si_(si), pis_(this), name_(name), setup_(false), msg_(name)
00045 {
00046 if (!si_)
00047 throw Exception(name_, "Invalid space information instance for planner");
00048 }
00049
00050 const ompl::base::PlannerSpecs& ompl::base::Planner::getSpecs(void) const
00051 {
00052 return specs_;
00053 }
00054
00055 const std::string& ompl::base::Planner::getName(void) const
00056 {
00057 return name_;
00058 }
00059
00060 void ompl::base::Planner::setName(const std::string &name)
00061 {
00062 name_ = name;
00063 msg_.setPrefix(name_);
00064 }
00065
00066 const ompl::base::SpaceInformationPtr& ompl::base::Planner::getSpaceInformation(void) const
00067 {
00068 return si_;
00069 }
00070
00071 const ompl::base::ProblemDefinitionPtr& ompl::base::Planner::getProblemDefinition(void) const
00072 {
00073 return pdef_;
00074 }
00075
00076 void ompl::base::Planner::setProblemDefinition(const ProblemDefinitionPtr &pdef)
00077 {
00078 pdef_ = pdef;
00079 pis_.update();
00080 }
00081
00082 const ompl::base::PlannerInputStates& ompl::base::Planner::getPlannerInputStates(void) const
00083 {
00084 return pis_;
00085 }
00086
00087 void ompl::base::Planner::setup(void)
00088 {
00089 if (!si_->isSetup())
00090 {
00091 msg_.inform("Space information setup was not yet called. Calling now.");
00092 si_->setup();
00093 }
00094
00095 if (setup_)
00096 msg_.warn("Planner setup called multiple times");
00097 else
00098 setup_ = true;
00099 }
00100
00101 void ompl::base::Planner::checkValidity(void)
00102 {
00103 if (!isSetup())
00104 setup();
00105 pis_.checkValidity();
00106 }
00107
00108 bool ompl::base::Planner::isSetup(void) const
00109 {
00110 return setup_;
00111 }
00112
00113 void ompl::base::Planner::clear(void)
00114 {
00115 pis_.clear();
00116 pis_.update();
00117 }
00118
00119 void ompl::base::Planner::getPlannerData(PlannerData &data) const
00120 {
00121 data.si = si_;
00122 }
00123
00124 bool ompl::base::Planner::solve(const PlannerTerminationConditionFn &ptc, double checkInterval)
00125 {
00126 return solve(PlannerThreadedTerminationCondition(ptc, checkInterval));
00127 }
00128
00130 namespace ompl
00131 {
00132
00133 static bool timePassed(const time::point &endTime)
00134 {
00135 return time::now() > endTime;
00136 }
00137 }
00139
00140 bool ompl::base::Planner::solve(double solveTime)
00141 {
00142 if (solveTime < 1.0)
00143 return solve(PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(solveTime))));
00144 else
00145 return solve(PlannerThreadedTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(solveTime)), std::min(solveTime / 100.0, 0.1)));
00146 }
00147
00148 void ompl::base::PlannerInputStates::clear(void)
00149 {
00150 if (tempState_)
00151 {
00152 si_->freeState(tempState_);
00153 tempState_ = NULL;
00154 }
00155 addedStartStates_ = 0;
00156 sampledGoalsCount_ = 0;
00157 pdef_ = NULL;
00158 si_ = NULL;
00159 }
00160
00161 void ompl::base::PlannerInputStates::restart(void)
00162 {
00163 addedStartStates_ = 0;
00164 sampledGoalsCount_ = 0;
00165 }
00166
00167 bool ompl::base::PlannerInputStates::update(void)
00168 {
00169 if (!planner_)
00170 throw Exception("No planner set for PlannerInputStates");
00171 return use(planner_->getSpaceInformation(), planner_->getProblemDefinition());
00172 }
00173
00174 void ompl::base::PlannerInputStates::checkValidity(void) const
00175 {
00176 std::string error;
00177
00178 if (!pdef_)
00179 error = "Problem definition not specified";
00180 else
00181 {
00182 if (pdef_->getStartStateCount() <= 0)
00183 error = "No start states specified";
00184 else
00185 if (!pdef_->getGoal())
00186 error = "No goal specified";
00187 }
00188
00189 if (!error.empty())
00190 {
00191 if (planner_)
00192 throw Exception(planner_->getName(), error);
00193 else
00194 throw Exception(error);
00195 }
00196 }
00197
00198 bool ompl::base::PlannerInputStates::use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef)
00199 {
00200 if (si && pdef)
00201 return use(si.get(), pdef.get());
00202 else
00203 {
00204 clear();
00205 return true;
00206 }
00207 }
00208
00209 bool ompl::base::PlannerInputStates::use(const SpaceInformation *si, const ProblemDefinition *pdef)
00210 {
00211 if (pdef_ != pdef || si_ != si)
00212 {
00213 clear();
00214 pdef_ = pdef;
00215 si_ = si;
00216 return true;
00217 }
00218 return false;
00219 }
00220
00221 const ompl::base::State* ompl::base::PlannerInputStates::nextStart(void)
00222 {
00223 if (pdef_ == NULL || si_ == NULL)
00224 {
00225 std::string error = "Missing space information or problem definition";
00226 if (planner_)
00227 throw Exception(planner_->getName(), error);
00228 else
00229 throw Exception(error);
00230 }
00231
00232 while (addedStartStates_ < pdef_->getStartStateCount())
00233 {
00234 const base::State *st = pdef_->getStartState(addedStartStates_);
00235 addedStartStates_++;
00236 bool bounds = si_->satisfiesBounds(st);
00237 bool valid = bounds ? si_->isValid(st) : false;
00238 if (bounds && valid)
00239 return st;
00240 else
00241 {
00242 msg::Interface msg(planner_ ? planner_->getName() : "");
00243 msg.warn("Skipping invalid start state (invalid %s)", bounds ? "state": "bounds");
00244 }
00245 }
00246 return NULL;
00247 }
00248
00249 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(void)
00250 {
00251 static PlannerAlwaysTerminatingCondition ptc;
00252 return nextGoal(ptc);
00253 }
00254
00255 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
00256 {
00257 if (pdef_ == NULL || si_ == NULL)
00258 {
00259 std::string error = "Missing space information or problem definition";
00260 if (planner_)
00261 throw Exception(planner_->getName(), error);
00262 else
00263 throw Exception(error);
00264 }
00265
00266 const GoalSampleableRegion *goal = dynamic_cast<const GoalSampleableRegion*>(pdef_->getGoal().get());
00267
00268 if (goal)
00269 {
00270 const GoalLazySamples *gls = dynamic_cast<const GoalLazySamples*>(goal);
00271 bool first = true;
00272 bool attempt = true;
00273 while (attempt)
00274 {
00275 attempt = false;
00276
00277 if (sampledGoalsCount_ < goal->maxSampleCount())
00278 {
00279 if (tempState_ == NULL)
00280 tempState_ = si_->allocState();
00281
00282 do
00283 {
00284 goal->sampleGoal(tempState_);
00285 sampledGoalsCount_++;
00286 bool bounds = si_->satisfiesBounds(tempState_);
00287 bool valid = bounds ? si_->isValid(tempState_) : false;
00288 if (bounds && valid)
00289 return tempState_;
00290 else
00291 {
00292 msg::Interface msg(planner_ ? planner_->getName() : "");
00293 msg.warn("Skipping invalid goal state (invalid %s)", bounds ? "state": "bounds");
00294 }
00295 }
00296 while (sampledGoalsCount_ < goal->maxSampleCount() && !ptc());
00297 }
00298
00299 if (gls && goal->canSample() && !ptc())
00300 {
00301 if (first)
00302 {
00303 first = false;
00304 msg::Interface msg(planner_ ? planner_->getName() : "");
00305 msg.debug("Waiting for goal region samples ...");
00306 }
00307 boost::this_thread::sleep(time::seconds(0.01));
00308 attempt = !ptc();
00309 }
00310 }
00311 }
00312 return NULL;
00313 }
00314
00315 bool ompl::base::PlannerInputStates::haveMoreStartStates(void) const
00316 {
00317 if (pdef_)
00318 return addedStartStates_ < pdef_->getStartStateCount();
00319 return false;
00320 }
00321
00322 bool ompl::base::PlannerInputStates::haveMoreGoalStates(void) const
00323 {
00324 if (pdef_ && pdef_->getGoal())
00325 {
00326 const GoalSampleableRegion *goal = dynamic_cast<const GoalSampleableRegion*>(pdef_->getGoal().get());
00327 if (goal)
00328 return sampledGoalsCount_ < goal->maxSampleCount();
00329 }
00330 return false;
00331 }