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