ompl/base/src/Planner.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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() const 00051 { 00052 return specs_; 00053 } 00054 00055 const std::string& ompl::base::Planner::getName() 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() const 00066 { 00067 return si_; 00068 } 00069 00070 const ompl::base::ProblemDefinitionPtr& ompl::base::Planner::getProblemDefinition() 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() const 00082 { 00083 return pis_; 00084 } 00085 00086 void ompl::base::Planner::setup() 00087 { 00088 if (!si_->isSetup()) 00089 { 00090 OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str()); 00091 si_->setup(); 00092 } 00093 00094 if (setup_) 00095 OMPL_WARN("%s: Planner setup called multiple times", getName().c_str()); 00096 else 00097 setup_ = true; 00098 } 00099 00100 void ompl::base::Planner::checkValidity() 00101 { 00102 if (!isSetup()) 00103 setup(); 00104 pis_.checkValidity(); 00105 } 00106 00107 bool ompl::base::Planner::isSetup() const 00108 { 00109 return setup_; 00110 } 00111 00112 void ompl::base::Planner::clear() 00113 { 00114 pis_.clear(); 00115 pis_.update(); 00116 } 00117 00118 void ompl::base::Planner::getPlannerData(PlannerData &data) const 00119 { 00120 for (PlannerProgressProperties::const_iterator it = plannerProgressProperties_.begin() ; it != plannerProgressProperties_.end() ; ++it) 00121 data.properties[it->first] = it->second(); 00122 } 00123 00124 ompl::base::PlannerStatus ompl::base::Planner::solve(const PlannerTerminationConditionFn &ptc, double checkInterval) 00125 { 00126 return solve(PlannerTerminationCondition(ptc, checkInterval)); 00127 } 00128 00129 ompl::base::PlannerStatus ompl::base::Planner::solve(double solveTime) 00130 { 00131 if (solveTime < 1.0) 00132 return solve(timedPlannerTerminationCondition(solveTime)); 00133 else 00134 return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1))); 00135 } 00136 00137 void ompl::base::Planner::printProperties(std::ostream &out) const 00138 { 00139 out << "Planner " + getName() + " specs:" << std::endl; 00140 out << "Multithreaded: " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl; 00141 out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl; 00142 out << "Can optimize solutions: " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl; 00143 out << "Aware of the following parameters:"; 00144 std::vector<std::string> params; 00145 params_.getParamNames(params); 00146 for (unsigned int i = 0 ; i < params.size() ; ++i) 00147 out << " " << params[i]; 00148 out << std::endl; 00149 } 00150 00151 void ompl::base::Planner::printSettings(std::ostream &out) const 00152 { 00153 out << "Declared parameters for planner " << getName() << ":" << std::endl; 00154 params_.print(out); 00155 } 00156 00157 void ompl::base::PlannerInputStates::clear() 00158 { 00159 if (tempState_) 00160 { 00161 si_->freeState(tempState_); 00162 tempState_ = NULL; 00163 } 00164 addedStartStates_ = 0; 00165 sampledGoalsCount_ = 0; 00166 pdef_ = NULL; 00167 si_ = NULL; 00168 } 00169 00170 void ompl::base::PlannerInputStates::restart() 00171 { 00172 addedStartStates_ = 0; 00173 sampledGoalsCount_ = 0; 00174 } 00175 00176 bool ompl::base::PlannerInputStates::update() 00177 { 00178 if (!planner_) 00179 throw Exception("No planner set for PlannerInputStates"); 00180 return use(planner_->getProblemDefinition()); 00181 } 00182 00183 void ompl::base::PlannerInputStates::checkValidity() const 00184 { 00185 std::string error; 00186 00187 if (!pdef_) 00188 error = "Problem definition not specified"; 00189 else 00190 { 00191 if (pdef_->getStartStateCount() <= 0) 00192 error = "No start states specified"; 00193 else 00194 if (!pdef_->getGoal()) 00195 error = "No goal specified"; 00196 } 00197 00198 if (!error.empty()) 00199 { 00200 if (planner_) 00201 throw Exception(planner_->getName(), error); 00202 else 00203 throw Exception(error); 00204 } 00205 } 00206 00207 bool ompl::base::PlannerInputStates::use(const ProblemDefinitionPtr &pdef) 00208 { 00209 if (pdef) 00210 return use(pdef.get()); 00211 else 00212 { 00213 clear(); 00214 return true; 00215 } 00216 } 00217 00218 bool ompl::base::PlannerInputStates::use(const ProblemDefinition *pdef) 00219 { 00220 if (pdef_ != pdef) 00221 { 00222 clear(); 00223 pdef_ = pdef; 00224 si_ = pdef->getSpaceInformation().get(); 00225 return true; 00226 } 00227 return false; 00228 } 00229 00230 const ompl::base::State* ompl::base::PlannerInputStates::nextStart() 00231 { 00232 if (pdef_ == NULL || si_ == NULL) 00233 { 00234 std::string error = "Missing space information or problem definition"; 00235 if (planner_) 00236 throw Exception(planner_->getName(), error); 00237 else 00238 throw Exception(error); 00239 } 00240 00241 while (addedStartStates_ < pdef_->getStartStateCount()) 00242 { 00243 const base::State *st = pdef_->getStartState(addedStartStates_); 00244 addedStartStates_++; 00245 bool bounds = si_->satisfiesBounds(st); 00246 bool valid = bounds ? si_->isValid(st) : false; 00247 if (bounds && valid) 00248 return st; 00249 else 00250 { 00251 OMPL_WARN("%s: Skipping invalid start state (invalid %s)", 00252 planner_ ? planner_->getName().c_str() : "PlannerInputStates", 00253 bounds ? "state": "bounds"); 00254 std::stringstream ss; 00255 si_->printState(st, ss); 00256 OMPL_DEBUG("%s: Discarded start state %s", 00257 planner_ ? planner_->getName().c_str() : "PlannerInputStates", 00258 ss.str().c_str()); 00259 } 00260 } 00261 return NULL; 00262 } 00263 00264 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal() 00265 { 00266 // This initialization is safe since we are in a non-const function anyway. 00267 static PlannerTerminationCondition ptc = plannerAlwaysTerminatingCondition(); 00268 return nextGoal(ptc); 00269 } 00270 00271 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc) 00272 { 00273 if (pdef_ == NULL || si_ == NULL) 00274 { 00275 std::string error = "Missing space information or problem definition"; 00276 if (planner_) 00277 throw Exception(planner_->getName(), error); 00278 else 00279 throw Exception(error); 00280 } 00281 00282 const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL; 00283 00284 if (goal) 00285 { 00286 time::point start_wait; 00287 bool first = true; 00288 bool attempt = true; 00289 while (attempt) 00290 { 00291 attempt = false; 00292 00293 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample()) 00294 { 00295 if (tempState_ == NULL) 00296 tempState_ = si_->allocState(); 00297 do 00298 { 00299 goal->sampleGoal(tempState_); 00300 sampledGoalsCount_++; 00301 bool bounds = si_->satisfiesBounds(tempState_); 00302 bool valid = bounds ? si_->isValid(tempState_) : false; 00303 if (bounds && valid) 00304 { 00305 if (!first) // if we waited, show how long 00306 { 00307 OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.", 00308 planner_ ? planner_->getName().c_str() : "PlannerInputStates", 00309 time::seconds(time::now() - start_wait)); 00310 } 00311 return tempState_; 00312 } 00313 else 00314 { 00315 OMPL_WARN("%s: Skipping invalid goal state (invalid %s)", 00316 planner_ ? planner_->getName().c_str() : "PlannerInputStates", 00317 bounds ? "state": "bounds"); 00318 std::stringstream ss; 00319 si_->printState(tempState_, ss); 00320 OMPL_DEBUG("%s: Discarded goal state %s", 00321 planner_ ? planner_->getName().c_str() : "PlannerInputStates", 00322 ss.str().c_str()); 00323 } 00324 } 00325 while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample()); 00326 } 00327 if (goal->couldSample() && !ptc) 00328 { 00329 if (first) 00330 { 00331 first = false; 00332 start_wait = time::now(); 00333 OMPL_DEBUG("%s: Waiting for goal region samples ...", 00334 planner_ ? planner_->getName().c_str() : "PlannerInputStates"); 00335 } 00336 boost::this_thread::sleep(time::seconds(0.01)); 00337 attempt = !ptc; 00338 } 00339 } 00340 } 00341 00342 return NULL; 00343 } 00344 00345 bool ompl::base::PlannerInputStates::haveMoreStartStates() const 00346 { 00347 if (pdef_) 00348 return addedStartStates_ < pdef_->getStartStateCount(); 00349 return false; 00350 } 00351 00352 bool ompl::base::PlannerInputStates::haveMoreGoalStates() const 00353 { 00354 if (pdef_ && pdef_->getGoal()) 00355 if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION)) 00356 return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount(); 00357 return false; 00358 }