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/goals/GoalSampleableRegion.h"
00040 #include <sstream>
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)
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 }
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 logInform("Space information setup was not yet called. Calling now.");
00091 si_->setup();
00092 }
00093
00094 if (setup_)
00095 logWarn("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 }
00121
00122 ompl::base::PlannerStatus ompl::base::Planner::solve(const PlannerTerminationConditionFn &ptc, double checkInterval)
00123 {
00124 return solve(PlannerThreadedTerminationCondition(ptc, checkInterval));
00125 }
00126
00127 ompl::base::PlannerStatus ompl::base::Planner::solve(double solveTime)
00128 {
00129 if (solveTime < 1.0)
00130 return solve(timedPlannerTerminationCondition(solveTime));
00131 else
00132 return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)));
00133 }
00134
00135 void ompl::base::Planner::printProperties(std::ostream &out) const
00136 {
00137 out << "Planner " + getName() + " specs:" << std::endl;
00138 out << "Multithreaded: " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl;
00139 out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl;
00140 out << "Can optimize solutions: " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl;
00141 out << "Aware of the following parameters:";
00142 std::vector<std::string> params;
00143 params_.getParamNames(params);
00144 for (unsigned int i = 0 ; i < params.size() ; ++i)
00145 out << " " << params[i];
00146 out << std::endl;
00147 }
00148
00149 void ompl::base::Planner::printSettings(std::ostream &out) const
00150 {
00151 out << "Declared parameters for planner " << getName() << ":" << std::endl;
00152 params_.print(out);
00153 }
00154
00155 void ompl::base::PlannerInputStates::clear(void)
00156 {
00157 if (tempState_)
00158 {
00159 si_->freeState(tempState_);
00160 tempState_ = NULL;
00161 }
00162 addedStartStates_ = 0;
00163 sampledGoalsCount_ = 0;
00164 pdef_ = NULL;
00165 si_ = NULL;
00166 }
00167
00168 void ompl::base::PlannerInputStates::restart(void)
00169 {
00170 addedStartStates_ = 0;
00171 sampledGoalsCount_ = 0;
00172 }
00173
00174 bool ompl::base::PlannerInputStates::update(void)
00175 {
00176 if (!planner_)
00177 throw Exception("No planner set for PlannerInputStates");
00178 return use(planner_->getSpaceInformation(), planner_->getProblemDefinition());
00179 }
00180
00181 void ompl::base::PlannerInputStates::checkValidity(void) const
00182 {
00183 std::string error;
00184
00185 if (!pdef_)
00186 error = "Problem definition not specified";
00187 else
00188 {
00189 if (pdef_->getStartStateCount() <= 0)
00190 error = "No start states specified";
00191 else
00192 if (!pdef_->getGoal())
00193 error = "No goal specified";
00194 }
00195
00196 if (!error.empty())
00197 {
00198 if (planner_)
00199 throw Exception(planner_->getName(), error);
00200 else
00201 throw Exception(error);
00202 }
00203 }
00204
00205 bool ompl::base::PlannerInputStates::use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef)
00206 {
00207 if (si && pdef)
00208 return use(si.get(), pdef.get());
00209 else
00210 {
00211 clear();
00212 return true;
00213 }
00214 }
00215
00216 bool ompl::base::PlannerInputStates::use(const SpaceInformation *si, const ProblemDefinition *pdef)
00217 {
00218 if (pdef_ != pdef || si_ != si)
00219 {
00220 clear();
00221 pdef_ = pdef;
00222 si_ = si;
00223 return true;
00224 }
00225 return false;
00226 }
00227
00228 const ompl::base::State* ompl::base::PlannerInputStates::nextStart(void)
00229 {
00230 if (pdef_ == NULL || si_ == NULL)
00231 {
00232 std::string error = "Missing space information or problem definition";
00233 if (planner_)
00234 throw Exception(planner_->getName(), error);
00235 else
00236 throw Exception(error);
00237 }
00238
00239 while (addedStartStates_ < pdef_->getStartStateCount())
00240 {
00241 const base::State *st = pdef_->getStartState(addedStartStates_);
00242 addedStartStates_++;
00243 bool bounds = si_->satisfiesBounds(st);
00244 bool valid = bounds ? si_->isValid(st) : false;
00245 if (bounds && valid)
00246 return st;
00247 else
00248 {
00249 logWarn("Skipping invalid start state (invalid %s)", bounds ? "state": "bounds");
00250 std::stringstream ss;
00251 si_->printState(st, ss);
00252 logDebug("Discarded start state %s", ss.str().c_str());
00253 }
00254 }
00255 return NULL;
00256 }
00257
00258 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(void)
00259 {
00260 static PlannerAlwaysTerminatingCondition ptc;
00261 return nextGoal(ptc);
00262 }
00263
00264 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
00265 {
00266 if (pdef_ == NULL || si_ == NULL)
00267 {
00268 std::string error = "Missing space information or problem definition";
00269 if (planner_)
00270 throw Exception(planner_->getName(), error);
00271 else
00272 throw Exception(error);
00273 }
00274
00275 const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
00276
00277 if (goal)
00278 {
00279 time::point start_wait;
00280 bool first = true;
00281 bool attempt = true;
00282 while (attempt)
00283 {
00284 attempt = false;
00285
00286 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
00287 {
00288 if (tempState_ == NULL)
00289 tempState_ = si_->allocState();
00290 do
00291 {
00292 goal->sampleGoal(tempState_);
00293 sampledGoalsCount_++;
00294 bool bounds = si_->satisfiesBounds(tempState_);
00295 bool valid = bounds ? si_->isValid(tempState_) : false;
00296 if (bounds && valid)
00297 {
00298 if (!first)
00299 {
00300 logDebug("Waited %lf seconds for the first goal sample.", time::seconds(time::now() - start_wait));
00301 }
00302 return tempState_;
00303 }
00304 else
00305 {
00306 logWarn("Skipping invalid goal state (invalid %s)", bounds ? "state": "bounds");
00307 std::stringstream ss;
00308 si_->printState(tempState_, ss);
00309 logDebug("Discarded goal state %s", ss.str().c_str());
00310 }
00311 }
00312 while (!ptc() && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
00313 }
00314 if (goal->couldSample() && !ptc())
00315 {
00316 if (first)
00317 {
00318 first = false;
00319 start_wait = time::now();
00320 logDebug("Waiting for goal region samples ...");
00321 }
00322 boost::this_thread::sleep(time::seconds(0.01));
00323 attempt = !ptc();
00324 }
00325 }
00326 }
00327
00328 return NULL;
00329 }
00330
00331 bool ompl::base::PlannerInputStates::haveMoreStartStates(void) const
00332 {
00333 if (pdef_)
00334 return addedStartStates_ < pdef_->getStartStateCount();
00335 return false;
00336 }
00337
00338 bool ompl::base::PlannerInputStates::haveMoreGoalStates(void) const
00339 {
00340 if (pdef_ && pdef_->getGoal())
00341 if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
00342 return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
00343 return false;
00344 }